(* Content-type: application/mathematica *)

(*** Wolfram Notebook File ***)
(* http://www.wolfram.com/nb *)

(* CreatedBy='Mathematica 7.0' *)

(*CacheID: 234*)
(* Internal cache information:
NotebookFileLineBreakTest
NotebookFileLineBreakTest
NotebookDataPosition[       145,          7]
NotebookDataLength[    296587,       7792]
NotebookOptionsPosition[    290306,       7605]
NotebookOutlinePosition[    290699,       7622]
CellTagsIndexPosition[    290656,       7619]
WindowFrame->Normal*)

(* Beginning of Notebook Content *)
Notebook[{
Cell[BoxData[
 RowBox[{
  RowBox[{"(*", "AMBER", "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{
    "SetDirectory", "[", 
     "\"\</home/shu/workspace/Research/AMBER_Output_Test/model\>\"", "]"}], 
    ";"}], "\n", 
   RowBox[{"<<", "RobotLinks.m"}], "\n", 
   RowBox[{"<<", "Linearize.m"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"SetDirectory", "[", 
      RowBox[{
      "\"\</home/shu/workspace/Research/AMBER_Output_Test/model/\>\"", "<>", 
       "\"\<build_torso\>\""}], "]"}], ";"}], "*)"}]}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"LineNumber", " ", "=", " ", "1"}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"IndexAllPre", " ", "=", " ", 
    RowBox[{"Import", "[", 
     RowBox[{"\"\<../data/IndexAll.mat\>\"", ",", "\"\<MAT\>\""}], "]"}]}], 
   ";"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"Dimensions", "[", "IndexAllPre", "]"}], ";"}], 
   "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{"IndexAll", "=", 
    RowBox[{"Join", "[", 
     RowBox[{"First", "[", "IndexAllPre", "]"}], "]"}]}], ";"}], "\n", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"Dimensions", "[", "IndexAll", "]"}], ";"}], "*)"}], 
  "\[IndentingNewLine]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"HipPosIndex", "=", 
   RowBox[{"Round", "[", 
    RowBox[{"IndexAll", "[", 
     RowBox[{"[", 
      RowBox[{"LineNumber", ",", "1"}], "]"}], "]"}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"NSslopeIndex", "=", 
   RowBox[{"Round", "[", 
    RowBox[{"IndexAll", "[", 
     RowBox[{"[", 
      RowBox[{"LineNumber", ",", "2"}], "]"}], "]"}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"TorsoIndex", "=", 
   RowBox[{"Round", "[", 
    RowBox[{"IndexAll", "[", 
     RowBox[{"[", 
      RowBox[{"LineNumber", ",", "3"}], "]"}], "]"}], "]"}]}], 
  ";"}]}], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, 
   3.51895445209264*^9, {3.519676671577675*^9, 3.519676676594407*^9}, {
   3.5239667982730017`*^9, 3.523966813334333*^9}, {3.523967028425743*^9, 
   3.523967032289135*^9}, {3.523967158902598*^9, 3.523967166280966*^9}, {
   3.52396837941966*^9, 3.523968408653638*^9}, {3.523968885862918*^9, 
   3.5239689132702827`*^9}, {3.523968985792946*^9, 3.5239689955887833`*^9}, {
   3.523969165241643*^9, 3.5239692240661287`*^9}, {3.5239694880148153`*^9, 
   3.523969499785118*^9}, {3.5239695714828377`*^9, 3.523969581546866*^9}, 
   3.52396965474053*^9, {3.523979540006937*^9, 3.523979540390903*^9}, 
   3.5243321051012077`*^9, {3.524333648683949*^9, 3.524333648941087*^9}, 
   3.52491991104259*^9, {3.526909166246196*^9, 3.5269091768319893`*^9}, {
   3.527022421428713*^9, 3.527022438914948*^9}, {3.527022726620986*^9, 
   3.5270227402575073`*^9}, 3.527027652455699*^9, {3.527027687003477*^9, 
   3.527027691012311*^9}, {3.527031346610958*^9, 3.527031347212298*^9}, {
   3.527075803741696*^9, 3.527075873329626*^9}, 3.5270759173702497`*^9, {
   3.527078036507852*^9, 3.527078050126071*^9}, {3.527197911583006*^9, 
   3.527197913230167*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   RowBox[{"t0", "=", 
    RowBox[{"AbsoluteTime", "[", "]"}]}], ";"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"model", " ", "specification"}], "*)"}]}], "\n", 
 RowBox[{
  RowBox[{"robotInfo", "=", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{
      "Lc", ",", "Lt", ",", "LT", ",", "rc", ",", "rt", ",", "rT", ",", "mh", 
       ",", "mt", ",", "mc", ",", "g"}], "}"}], ",", "\[IndentingNewLine]", 
     RowBox[{"{", 
      RowBox[{
      "347.98", ",", "261.112", ",", "0", ",", "282.37", ",", "128.15", ",", 
       "9.97", ",", 
       StyleBox["804.83",
        FontWeight->"Plain"], ",", "606.15", ",", "213.79", ",", "9810"}], 
      "}"}]}], "}"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"constsubs", "=", 
   RowBox[{
    RowBox[{"(", 
     RowBox[{
      RowBox[{
       RowBox[{"#1", "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}], 
       "\[Rule]", 
       RowBox[{"Rationalize", "[", 
        FractionBox[
         RowBox[{"#1", "\[LeftDoubleBracket]", "2", "\[RightDoubleBracket]"}],
          "1000"], "]"}]}], "&"}], ")"}], "/@", 
    RowBox[{"Transpose", "[", "robotInfo", "]"}]}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"ndof", " ", "=", " ", "5"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"mm", "=", 
   RowBox[{
    RowBox[{"{", 
     RowBox[{"mc", ",", "mt", ",", "mh", ",", "mt", ",", "mc"}], "}"}], "/.", 
    "\[InvisibleSpace]", "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"statesubs", "=", 
    RowBox[{"Join", "[", 
     RowBox[{
      RowBox[{"Table", "[", 
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "\[Rule]", 
         RowBox[{"x", "[", "i", "]"}]}], ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}], ",", 
      RowBox[{"Table", "[", 
       RowBox[{
        RowBox[{
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "i"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
         RowBox[{"x", "[", 
          RowBox[{"i", "+", "ndof"}], "]"}]}], ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}]}], "]"}]}], 
   ";"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"define", " ", "inertia", " ", "matrices"}], "*)"}]}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"For", "[", 
    RowBox[{
     RowBox[{"i", "=", "1"}], ",", 
     RowBox[{"i", "\[LessEqual]", 
      RowBox[{"Length", "[", "mm", "]"}]}], ",", 
     RowBox[{"i", "++"}], ",", 
     RowBox[{
      SubscriptBox["\[ScriptCapitalM]", "i"], "=", 
      RowBox[{
       RowBox[{"mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
       " ", 
       RowBox[{"DiagonalMatrix", "[", 
        RowBox[{"{", 
         RowBox[{"1", ",", "1", ",", "1"}], "}"}], "]"}]}]}]}], "]"}], ";"}], 
  "\n", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"add", " ", "inertia", " ", "for", " ", "torso", " ", 
     RowBox[{"(", 
      RowBox[{"2.5", " ", "cm", " ", "radius"}], ")"}]}], ",", 
    RowBox[{"the", " ", "x"}], ",", 
    RowBox[{
    "z", " ", "terms", " ", "will", " ", "not", " ", "show", " ", "up", " ", 
     "in", " ", "the", " ", "EOM", " ", "for", " ", "this", " ", "2", "d", 
     " ", "model"}]}], "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "1"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"1967374.33", "0", "12.37"},
         {"0", "1946798.09", 
          RowBox[{"-", "19.96"}]},
         {"12.37", 
          RowBox[{"-", "19.96"}], "119696.29"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "2"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"6494948.89", "58.80", "2024.23"},
         {"58.80", "6396011.40", "146697.91"},
         {"2024.23", "146697.91", "418371.07"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "3"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"3730232.81", 
          RowBox[{"-", "7.44"}], "1948.96"},
         {
          RowBox[{"-", "7.44"}], "518271.83", 
          RowBox[{"-", "1348.28"}]},
         {"1948.96", 
          RowBox[{"-", "1348.28"}], "3577190.08"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "4"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"6494948.89", 
          RowBox[{"-", "60.11"}], "1985.63"},
         {
          RowBox[{"-", "60.11"}], "6396011.38", 
          RowBox[{"-", "146697.91"}]},
         {"1985.63", 
          RowBox[{"-", "146697.91"}], "418371.06"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "5"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"1967374.33", "0", 
          RowBox[{"-", "12.37"}]},
         {"0", "1946798.091", "19.96"},
         {
          RowBox[{"-", "12.37"}], "19.96", "119696.29"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{"For", "[", 
  RowBox[{
   RowBox[{"i", "=", "1"}], ",", 
   RowBox[{"i", "\[LessEqual]", "5"}], ",", 
   RowBox[{"i", "++"}], ",", 
   RowBox[{
    SubscriptBox["\[ScriptCapitalM]", "i"], "=", 
    RowBox[{"Join", "[", 
     RowBox[{
      RowBox[{"Join", "[", 
       RowBox[{
        SubscriptBox["\[ScriptCapitalM]", "i"], ",", 
        RowBox[{"Table", "[", 
         RowBox[{"0", ",", 
          RowBox[{"{", "3", "}"}], ",", 
          RowBox[{"{", "3", "}"}]}], "]"}], ",", "2"}], "]"}], ",", 
      RowBox[{"Join", "[", 
       RowBox[{
        RowBox[{"Table", "[", 
         RowBox[{"0", ",", 
          RowBox[{"{", "3", "}"}], ",", 
          RowBox[{"{", "3", "}"}]}], "]"}], ",", 
        SubscriptBox["\[ScriptCapitalI]", "i"], ",", "2"}], "]"}], ",", "1"}],
      "]"}]}]}], "]"}], "\n", 
 RowBox[{
  RowBox[{"p0", "=", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["p", "x"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["p", "z"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["p", "x"], "'"}], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["p", "z"], "'"}], "[", "t", "]"}], "\[Rule]", "0"}]}], 
    "}"}]}], ";"}]}], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"generalized", " ", "coordinates"}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"q", "=", 
     RowBox[{"Table", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"dq", "=", 
     RowBox[{
      SubscriptBox["\[PartialD]", "t"], "q"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"ddq", "=", 
     RowBox[{
      SubscriptBox["\[PartialD]", "t"], "dq"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"qe", "=", 
     RowBox[{"Join", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}], "}"}]}], "}"}], ",", 
       "q"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"dqe", "=", 
     RowBox[{
      SubscriptBox["\[PartialD]", "t"], "qe"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
    "location", " ", "and", " ", "direction", " ", "of", " ", "twists"}], 
    "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "0"], "=", 
     RowBox[{"{", 
      RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], 
      "}"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "px"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"PrismaticTwist", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"1", ",", "0", ",", "0"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "pz"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"PrismaticTwist", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q1"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RevoluteTwist", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", 
           RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q2"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RevoluteTwist", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "Lc"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", 
           RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q3"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RevoluteTwist", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", 
           RowBox[{"Lc", "+", "Lt"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", 
           RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q4"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RevoluteTwist", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", 
           RowBox[{"Lc", "+", "Lt"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q5"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RevoluteTwist", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "Lc"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"base", " ", "configuration"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "1"]], "[", "0", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RPToHomogeneous", "[", 
        RowBox[{
         RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", 
           FractionBox[
            RowBox[{"-", "33"}], "100000"], ",", "rc"}], "}"}]}], "]"}], "/.",
        "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "2"]], "[", "0", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RPToHomogeneous", "[", 
        RowBox[{
         RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{
           FractionBox["2", "100000"], ",", 
           FractionBox[
            RowBox[{"-", "381"}], "100000"], ",", 
           RowBox[{"Lc", "+", "rt"}]}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "3"]], "[", "0", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RPToHomogeneous", "[", 
        RowBox[{
         RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{
           FractionBox[
            RowBox[{"-", "24"}], "100000"], ",", 
           FractionBox[
            RowBox[{"-", "4"}], "100000"], ",", 
           RowBox[{"Lc", "+", "Lt", "+", "rT"}]}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "4"]], "[", "0", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RPToHomogeneous", "[", 
        RowBox[{
         RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{
           FractionBox[
            RowBox[{"-", "24"}], "100000"], ",", 
           FractionBox[
            RowBox[{"-", "4"}], "100000"], ",", 
           RowBox[{"Lc", "+", "Lt", "+", "Lb"}]}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "4"]], "[", "0", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RPToHomogeneous", "[", 
        RowBox[{
         RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{
           FractionBox["2", "100000"], ",", 
           FractionBox["381", "100000"], ",", 
           RowBox[{"Lc", "+", "rt"}]}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "5"]], "[", "0", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RPToHomogeneous", "[", 
        RowBox[{
         RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", 
           FractionBox["33", "100000"], ",", "rc"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalJ]", "1"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"BodyJacobian", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "1"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalJ]", "2"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"BodyJacobian", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "2"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalJ]", "3"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"BodyJacobian", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "3"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalJ]", "4"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"BodyJacobian", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "4"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalJ]", "5"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"BodyJacobian", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q5"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "5"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "forward", " ", "kinematics", " ", "maps"}],
     "*)"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "1"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "px"], ",", 
           RowBox[{
            SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "pz"], ",", 
           RowBox[{
            SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", 
           SubscriptBox["sl", "1"]], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "2"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "px"], ",", 
           RowBox[{
            SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "pz"], ",", 
           RowBox[{
            SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", 
           SubscriptBox["sl", "2"]], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "3"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "px"], ",", 
           RowBox[{
            SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "pz"], ",", 
           RowBox[{
            SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q3"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", 
           SubscriptBox["sl", "3"]], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "4"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "px"], ",", 
           RowBox[{
            SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "pz"], ",", 
           RowBox[{
            SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q3"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q4"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", 
           SubscriptBox["sl", "4"]], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "5"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "px"], ",", 
           RowBox[{
            SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "pz"], ",", 
           RowBox[{
            SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q3"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q4"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q5"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", 
           SubscriptBox["sl", "5"]], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "stf"], "[", "0", "]"}], "=", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}], "/.", 
      "constsubs"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "stk"], "[", "0", "]"}], "=", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "Lc"}], "}"}]}], "]"}], "/.", 
      "constsubs"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "hip"], "[", "0", "]"}], "=", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", 
          RowBox[{"Lc", "+", "Lt"}]}], "}"}]}], "]"}], "/.", "constsubs"}]}], 
    ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "torso"], "[", "0", "]"}], "=", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", 
          RowBox[{"Lc", "+", "Lt", "+", "LT"}]}], "}"}]}], "]"}], "/.", 
      "constsubs"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsk"], "[", "0", "]"}], "=", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "Lc"}], "}"}]}], "]"}], "/.", 
      "constsubs"}]}], ";", 
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "0", "]"}], "=", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}], "/.", 
      "constsubs"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "0"], ",", "0"}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", "stf"], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "stk"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", "stk"], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", "hip"], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q3"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", "torso"], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsk"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q3"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q4"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", "nsk"], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";", 
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q3"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q4"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q5"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{"(*", "aniplot", "*)"}], "\n", 
   RowBox[{
    RowBox[{"pos", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"Join", "[", 
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", 
          RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "stk"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", 
          RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", 
          RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", 
          RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", 
          RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsk"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", 
          RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", 
          RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", "2"}], 
       "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", "\n", 
   RowBox[{"(*", 
    RowBox[{"calculate", " ", "center", " ", "of", " ", "mass"}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["p", "COM"], "=", 
     RowBox[{
      RowBox[{
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          UnderoverscriptBox["\[Sum]", 
           RowBox[{"i", "=", "1"}], 
           RowBox[{"Length", "[", "mm", "]"}]], 
          RowBox[{
           RowBox[{
           "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], " ", 
           
           RowBox[{
            RowBox[{
             RowBox[{
              SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
             "\[LeftDoubleBracket]", 
             RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/", 
            RowBox[{"(", 
             RowBox[{"Total", "[", "mm", "]"}], ")"}]}]}]}], ")"}], "/.", 
        "constsubs"}], "//.", "p0"}], "//", "Simplify"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "manipulator", " ", "inertia", " ", 
     "matrix"}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"\[ScriptCapitalD]e", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       UnderoverscriptBox["\[Sum]", 
        RowBox[{"i", "=", "1"}], 
        RowBox[{"Length", "[", "mm", "]"}]], 
       RowBox[{
        RowBox[{
         SubscriptBox["\[ScriptCapitalJ]", "i"], "\[Transpose]"}], ".", 
        SubscriptBox["\[ScriptCapitalM]", "i"], ".", 
        SubscriptBox["\[ScriptCapitalJ]", "i"]}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
    "project", " ", "out", " ", "the", " ", "generalized", " ", "coordinates",
      " ", "defining", " ", "the", " ", "position", " ", "of", " ", "the", 
     " ", "stance", " ", "foot", " ", "to", " ", "obtain", " ", "the", " ", 
     "reduced", " ", "\[ScriptCapitalD]", " ", "matrix"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalD]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"\[ScriptCapitalD]e", "\[LeftDoubleBracket]", 
        RowBox[{
         RowBox[{"3", ";;", "All"}], ",", 
         RowBox[{"3", ";;", "All"}]}], "\[RightDoubleBracket]"}], "/.", 
       "\[InvisibleSpace]", "p0"}], "]"}]}], ";"}]}]}]], "Input",
 CellChangeTimes->{{3.504391169851249*^9, 3.504391191970865*^9}, {
   3.5129535326917257`*^9, 3.5129535355376797`*^9}, 3.513011937181409*^9, {
   3.513012138013254*^9, 3.513012139016991*^9}, 3.515858456069166*^9, {
   3.5158695182095623`*^9, 3.515869519316625*^9}, {3.52719507570704*^9, 
   3.527195080589549*^9}, {3.527195127333808*^9, 3.5271951339919443`*^9}, 
   3.527195290523535*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"calculate", " ", "Coriolis", " ", "matrix"}], "*)"}], 
  "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"\[ScriptCapitalC]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"InertiaToCoriolis", "[", 
       RowBox[{"\[ScriptCapitalD]", ",", 
        RowBox[{"Flatten", "[", "q", "]"}], ",", 
        RowBox[{"Flatten", "[", "dq", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "potential", " ", "energy", " ", "and", " ",
      "\[ScriptCapitalG]", " ", "matrix"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"V", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"g", " ", 
        RowBox[{
         UnderoverscriptBox["\[Sum]", 
          RowBox[{"i", "=", "1"}], 
          RowBox[{"Length", "[", "mm", "]"}]], 
         RowBox[{
          RowBox[{
          "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], " ", 
          RowBox[{
           RowBox[{
            SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
           "\[LeftDoubleBracket]", 
           RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]}]}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalG]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{"q", ",", "1"}], "}"}]], "V"}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "\[ScriptCapitalE]", " ", "matrix", " ", 
     "for", " ", "impact", " ", "and", " ", "the", " ", "guard"}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"the", " ", "position", " ", "of", " ", "swing", " ", "foot"}], 
    "*)"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q5"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalE]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "qe", "]"}], ",", "1"}], "}"}]], 
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1", ",", "3"}], "}"}], ",", "4"}], 
        "\[RightDoubleBracket]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"\[ScriptCapitalE]", "/.", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", ".2345"}],
        ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", ".1894"}],
        ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"-", ".293"}]}], ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", ".094"}], 
       ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"-", ".210"}]}], ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", 
        ".923"}]}], "}"}]}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"h", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "/.", 
       "\[InvisibleSpace]", "p0"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"hdot", " ", "=", "  ", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", "h", "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"hip", " ", "position", " ", "related"}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"1", " ", "Hip", " ", "Position"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"HipPos", " ", "=", " ", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/.", "p0"}]}], 
    ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"2", " ", "Linearized", " ", "Hip", " ", "Position"}], "*)"}], 
   "\n", 
   RowBox[{
    RowBox[{"LHipPos", " ", "=", " ", 
     RowBox[{"Linearize", "[", 
      RowBox[{"HipPos", ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"non", "-", 
     RowBox[{"stance", " ", "slope", " ", "related"}]}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"1", " ", "non"}], "-", 
     RowBox[{"stance", " ", "slope"}]}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"nsslope", " ", "=", " ", 
     RowBox[{
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], 
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]], "/.", 
      "p0"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"2.", " ", "linearized", " ", "non"}], "-", 
     RowBox[{"stance", " ", "slope"}]}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"LinearNSslope", "=", 
     RowBox[{"Linearize", "[", 
      RowBox[{
       RowBox[{"nsslope", " ", "/.", "p0"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"3.", " ", "hip", " ", "angle"}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"HipAngle", " ", "=", " ", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
      RowBox[{
       SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"torso", " ", "related"}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"1", " ", "Torso", " ", "non"}], "-", 
     RowBox[{"stance", " ", "thigh", " ", "angle"}]}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"theta4", " ", "=", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"2", " ", "Torso", " ", "Hip", " ", "angle"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"TorsoHipAngle", "=", " ", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"3", " ", "Torso", " ", "Non"}], "-", 
     RowBox[{"stance", " ", "Slope"}]}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"nstorso", " ", "=", " ", 
     FractionBox[
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
       RowBox[{
        RowBox[{
         SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
       RowBox[{
        RowBox[{
         SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"4", " ", "Linearized", " ", "torso", " ", "non"}], "-", 
     RowBox[{"stance", " ", "slope", " ", "angle"}]}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"LinearNStorso", "=", 
     RowBox[{"Linearize", "[", 
      RowBox[{
       RowBox[{"nstorso", " ", "/.", "p0"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"5", " ", "Torso", " ", "stance", " ", "slope"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"storso", " ", "=", " ", 
     FractionBox[
      RowBox[{
       RowBox[{
        SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], 
      RowBox[{
       RowBox[{
        SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
    "6", " ", "Linearized", " ", "Torso", " ", "stance", " ", "slope"}], 
    "*)"}], "\n", 
   RowBox[{
    RowBox[{"LinearStorso", "=", 
     RowBox[{"Linearize", "[", 
      RowBox[{
       RowBox[{"storso", " ", "/.", "p0"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"7", " ", "COM"}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["p", "COM"], "=", 
     RowBox[{
      RowBox[{
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          UnderoverscriptBox["\[Sum]", 
           RowBox[{"i", "=", "1"}], 
           RowBox[{"Length", "[", "mm", "]"}]], 
          RowBox[{
           RowBox[{
           "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
           RowBox[{
            RowBox[{
             RowBox[{
              SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
             "\[LeftDoubleBracket]", 
             RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"2", "*", 
               RowBox[{"(", 
                RowBox[{"mc", "+", "mt"}], ")"}]}], "+", "mh"}], ")"}]}]}]}], 
         "/.", "constsubs"}], ")"}], "//.", "p0"}], "//", "Simplify"}]}], 
    ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"8", " ", "Linearized", " ", "COM"}], "*)"}], 
   RowBox[{
    RowBox[{
     SubscriptBox["Linearp", "COM"], "=", " ", 
     RowBox[{"Linearize", "[", 
      RowBox[{
       RowBox[{
        SubscriptBox["p", "COM"], "/.", "p0"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", "\n"}]}]], "Input",
 CellChangeTimes->{
  3.50439255557097*^9, {3.51295513827149*^9, 3.512955172679001*^9}, {
   3.5129552037364693`*^9, 3.512955231006535*^9}, {3.512955263739307*^9, 
   3.5129553328924294`*^9}, {3.512992901952466*^9, 3.512992920307856*^9}, {
   3.5129940359160023`*^9, 3.512994058469393*^9}, 3.5129940928271093`*^9, {
   3.512994138765036*^9, 3.5129941583899803`*^9}, {3.51301260617554*^9, 
   3.513012607311402*^9}, {3.513012641937742*^9, 3.513012652342953*^9}, {
   3.513012738154763*^9, 3.513012748062248*^9}, 3.513012780026892*^9, {
   3.51301313424288*^9, 3.513013136853251*^9}, {3.527195306779914*^9, 
   3.527195310640623*^9}, {3.5271967036241007`*^9, 3.527196723980693*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"feedback", " ", "control"}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"\[Chi]", "=", 
     RowBox[{"Join", "[", 
      RowBox[{"q", ",", "dq"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"d\[Chi]", "=", 
     RowBox[{"D", "[", 
      RowBox[{"\[Chi]", ",", "t"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"hipOutput", " ", "=", " ", 
     RowBox[{"{", 
      RowBox[{"HipPos", ",", "LHipPos"}], "}"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["p", "hip"], " ", "=", 
     RowBox[{"hipOutput", "[", 
      RowBox[{"[", "HipPosIndex", "]"}], "]"}]}], " ", ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["p", "hipdot"], "  ", "=", " ", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["p", "hip"], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["v", "hip"], "=", 
     RowBox[{
      RowBox[{"D", "[", 
       RowBox[{
        SubscriptBox["p", "hip"], ",", "t"}], "]"}], "//", "Simplify"}]}], 
    ";"}], "\n", 
   RowBox[{
    RowBox[{"fb", "=", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"\[Sigma]", "[", "t", "]"}], "\[Rule]", 
       FractionBox[
        RowBox[{
         SubscriptBox["p", "hip"], "-", 
         RowBox[{"p", "[", "1", "]"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]}], "}"}]}], ";", 
    RowBox[{"(*", 
     RowBox[{"time", "-", 
      RowBox[{"invariant", " ", 
       RowBox[{"parameterization", ":"}]}]}], "*)"}], "\n", 
    RowBox[{"\[Sigma]y", "=", 
     RowBox[{
      RowBox[{"\[Sigma]", "[", "t", "]"}], "/.", "fb"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{"HumanFunction", "[", "i_", "]"}], ":=", 
     RowBox[{
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"i", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"i", ",", "2"}], "]"}], " ", 
           RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"i", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"i", ",", "2"}], "]"}], " ", 
           RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]}]}], 
       RowBox[{"Exp", "[", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"i", ",", "4"}], "]"}], 
         RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]], "+", 
      RowBox[{"a", "[", 
       RowBox[{"i", ",", "5"}], "]"}]}]}], ";", 
    RowBox[{"(*", 
     RowBox[{"calculate", 
      FormBox[
       SubscriptBox["y", "d"],
       TraditionalForm], "and", " ", "its", " ", "derivatives"}], "*)"}], 
    "\n", "\n", 
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"d", ",", "1"}]], " ", "=", " ", 
     RowBox[{"a", "[", 
      RowBox[{"1", ",", "1"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"d", ",", "2"}]], " ", "=", " ", 
     RowBox[{
      RowBox[{"Transpose", "[", 
       RowBox[{"{", 
        RowBox[{"Table", "[", 
         RowBox[{
          RowBox[{"HumanFunction", "[", "i", "]"}], ",", 
          RowBox[{"{", 
           RowBox[{"i", ",", " ", "2", ",", "ndof"}], "}"}]}], "]"}], "}"}], 
       "]"}], "/.", "fb"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"d", ",", "1"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", 
         RowBox[{"d", ",", "1"}]], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"d", ",", "2"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", 
         RowBox[{"d", ",", "2"}]], "]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"d", ",", "1"}]], "=", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        RowBox[{
         SubscriptBox["Dy", 
          RowBox[{"d", ",", "1"}]], ".", "d\[Chi]"}], "]"}]}], "//", 
      "Simplify"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"d", ",", "2"}]], "=", 
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"Flatten", "[", 
       RowBox[{
        SubscriptBox["Dy", 
         RowBox[{"d", ",", "2"}]], ".", "d\[Chi]"}], "]"}]}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "actual", " ", "kinematics", " ", "outputs", " ", "and",
      " ", "Jacobians"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"a", ",", "1"}]], "=", 
     RowBox[{"D", "[", 
      RowBox[{
       SubscriptBox["p", "hip"], ",", "t"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"NSslopeOutput", " ", "=", " ", 
     RowBox[{"{", 
      RowBox[{"nsslope", ",", "LinearNSslope", ",", "HipAngle"}], "}"}]}], 
    ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"TorsoOutput", " ", "=", " ", 
     RowBox[{"{", 
      RowBox[{
      "TorsoHipAngle", ",", "theta4", ",", "nstorso", ",", "LinearNStorso", 
       ",", "storso", ",", "LinearStorso", ",", 
       SubscriptBox["p", "COM"], ",", 
       SubscriptBox["Linearp", "COM"]}], "}"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"a", ",", "2"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"NSslopeOutput", "[", 
            RowBox[{"[", "NSslopeIndex", "]"}], "]"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{"TorsoOutput", " ", "[", 
            RowBox[{"[", "TorsoIndex", "]"}], "]"}], "}"}]}], "}"}], "/.", 
        "constsubs"}], "/.", "p0"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"a", ",", "1"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", 
         RowBox[{"a", ",", "1"}]], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"a", ",", "2"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", 
         RowBox[{"a", ",", "2"}]], "]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"a", ",", "1"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        RowBox[{
         SubscriptBox["Dy", 
          RowBox[{"a", ",", "1"}]], ".", "d\[Chi]"}], "]"}]}], "]"}]}], ";"}],
    "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"a", ",", "2"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        RowBox[{
         SubscriptBox["Dy", 
          RowBox[{"a", ",", "2"}]], ".", "d\[Chi]"}], "]"}]}], "]"}]}], ";"}],
    "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.513533323577435*^9, 3.513533331092033*^9}, {
   3.513533381181343*^9, 3.513533384041068*^9}, {3.513534180527118*^9, 
   3.513534186497912*^9}, {3.513534242697953*^9, 3.513534243136149*^9}, {
   3.527196798384557*^9, 3.527196802874881*^9}, 3.52719779898011*^9}],

Cell[BoxData[
 RowBox[{"(*", 
  RowBox[{
   RowBox[{
    SubscriptBox["p", "hip"], " ", "=", " ", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/.", "p0"}]}], ";",
    "\[IndentingNewLine]", 
   RowBox[{
    SubscriptBox["p", "hipdot"], "  ", "=", " ", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"Flatten", "[", 
       SubscriptBox["p", "hip"], "]"}]}], "]"}]}], ";", "\n", 
   RowBox[{"hipOutput", " ", "=", " ", 
    RowBox[{"{", 
     RowBox[{"HipPos", ",", "LHipPos", ",", "LHipPosD"}], "}"}]}], ";", 
   "\[IndentingNewLine]", 
   RowBox[{
    SubscriptBox["deltap", "hip"], " ", "=", 
    RowBox[{"hipOutput", "[", 
     RowBox[{"[", "2", "]"}], "]"}]}], ";", "\[IndentingNewLine]", 
   RowBox[{
    SubscriptBox["deltap", "hipdot"], "  ", "=", " ", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"Flatten", "[", 
       SubscriptBox["deltap", "hip"], "]"}]}], "]"}]}], ";", 
   "\[IndentingNewLine]", 
   RowBox[{
    SubscriptBox["deltav", "hip"], "=", 
    RowBox[{
     RowBox[{"D", "[", 
      RowBox[{
       SubscriptBox["deltap", "hip"], ",", "t"}], "]"}], "//", "Simplify"}]}],
    ";", "\n", 
   RowBox[{"fb", "=", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"\[Sigma]", "[", "t", "]"}], "\[Rule]", 
      FractionBox[
       RowBox[{
        SubscriptBox["deltap", "hip"], "-", 
        RowBox[{"p", "[", "1", "]"}]}], 
       RowBox[{"a", "[", 
        RowBox[{"1", ",", "1"}], "]"}]]}], "}"}]}], ";", "\n", 
   RowBox[{"\[Sigma]y", "=", 
    RowBox[{
     RowBox[{"\[Sigma]", "[", "t", "]"}], "/.", "fb"}]}], ";"}], 
  "*)"}]], "Input",
 CellChangeTimes->{{3.520119821955995*^9, 3.5201198226710362`*^9}, {
  3.5201773456230736`*^9, 3.52017734590409*^9}, {3.527197052406234*^9, 
  3.5271970545001707`*^9}, {3.5271971130092497`*^9, 3.527197125055758*^9}}],

Cell[BoxData[
 RowBox[{"(*", 
  RowBox[{
   RowBox[{
    RowBox[{"HumanFunction", "[", "i_", "]"}], ":=", 
    RowBox[{
     FractionBox[
      RowBox[{
       RowBox[{
        RowBox[{"a", "[", 
         RowBox[{"i", ",", "1"}], "]"}], " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"i", ",", "2"}], "]"}], " ", 
          RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]}], "+", 
       RowBox[{
        RowBox[{"a", "[", 
         RowBox[{"i", ",", "3"}], "]"}], " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"i", ",", "2"}], "]"}], " ", 
          RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]}]}], 
      RowBox[{"Exp", "[", 
       RowBox[{
        RowBox[{"a", "[", 
         RowBox[{"i", ",", "4"}], "]"}], 
        RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]], "+", 
     RowBox[{"a", "[", 
      RowBox[{"i", ",", "5"}], "]"}]}]}], ";", "\n", 
   RowBox[{
    SubscriptBox["y", 
     RowBox[{"d", ",", "1"}]], " ", "=", " ", 
    RowBox[{
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"1", ",", "1"}], "]"}], "\[IndentingNewLine]", 
      SubscriptBox["y", 
       RowBox[{"d", ",", "2"}]]}], " ", "=", " ", 
     RowBox[{
      RowBox[{"Transpose", "[", 
       RowBox[{"{", 
        RowBox[{"Table", "[", 
         RowBox[{
          RowBox[{"HumanFunction", "[", "i", "]"}], ",", 
          RowBox[{"{", 
           RowBox[{"i", ",", " ", "2", ",", "ndof"}], "}"}]}], "]"}], "}"}], 
       "]"}], "/.", "fb"}]}]}], ";", "\n", 
   RowBox[{
    RowBox[{
     RowBox[{"a", "[", 
      RowBox[{"1", ",", "1"}], "]"}], "\n", 
     SubscriptBox["Dy", 
      RowBox[{"d", ",", "1"}]]}], "=", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"Flatten", "[", 
       SubscriptBox["y", 
        RowBox[{"d", ",", "1"}]], "]"}]}], "]"}]}], ";", 
   "\[IndentingNewLine]", 
   RowBox[{
    SubscriptBox["Dy", 
     RowBox[{"d", ",", "2"}]], "=", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"Flatten", "[", 
       SubscriptBox["y", 
        RowBox[{"d", ",", "2"}]], "]"}]}], "]"}]}], ";", "\n", 
   RowBox[{
    SubscriptBox["DLfy", 
     RowBox[{"d", ",", "1"}]], "=", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"Flatten", "[", 
       RowBox[{
        SubscriptBox["Dy", 
         RowBox[{"d", ",", "1"}]], ".", "d\[Chi]"}], "]"}]}], "//", 
     "Simplify"}]}], ";", "\[IndentingNewLine]", 
   RowBox[{
    SubscriptBox["DLfy", 
     RowBox[{"d", ",", "2"}]], "=", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"Flatten", "[", 
       RowBox[{
        SubscriptBox["Dy", 
         RowBox[{"d", ",", "2"}]], ".", "d\[Chi]"}], "]"}]}], "//", 
     "Simplify"}]}], ";"}], "*)"}]], "Input",
 CellChangeTimes->{{3.515860352077612*^9, 3.5158603885236964`*^9}, {
  3.527197141988551*^9, 3.527197155523426*^9}}],

Cell[BoxData[
 RowBox[{"(*", 
  RowBox[{
   RowBox[{
    SubscriptBox["y", 
     RowBox[{"a", ",", "1"}]], "=", 
    RowBox[{
     SubscriptBox["deltav", "hip"], "\n", 
     RowBox[{"NSslopeOutput", " ", "=", " ", 
      RowBox[{"{", 
       RowBox[{"nsslope", ",", "LinearNSslope", ",", "HipAngle"}], 
       "}"}]}]}]}], ";", "\n", " ", "\[IndentingNewLine]", 
   RowBox[{"TorsoOutput", " ", "=", " ", 
    RowBox[{"{", 
     RowBox[{
     "TorsoHipAngle", ",", "theta4", ",", "nstorso", ",", "LinearNStorso", 
      ",", "storso", ",", "LinearStorso"}], "}"}]}], ";", "\n", 
   RowBox[{
    SubscriptBox["y", 
     RowBox[{"a", ",", "2"}]], "=", 
    RowBox[{
     RowBox[{
      RowBox[{"Simplify", "[", 
       RowBox[{
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           RowBox[{"{", 
            RowBox[{"NSslopeOutput", "[", 
             RowBox[{"[", "2", "]"}], "]"}], "}"}], ",", 
           RowBox[{"{", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "}"}], ",", 
           RowBox[{"{", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "}"}], ",", 
           RowBox[{"{", 
            RowBox[{"TorsoOutput", " ", "[", 
             RowBox[{"[", "1", "]"}], "]"}], "}"}]}], "}"}], "/.", 
         "constsubs"}], "/.", "p0"}], "]"}], "\n", 
      SubscriptBox["Dy", 
       RowBox[{"a", ",", "1"}]]}], "=", 
     RowBox[{
      RowBox[{
       RowBox[{"Simplify", "[", 
        RowBox[{
         SubscriptBox["\[PartialD]", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
         RowBox[{"Flatten", "[", 
          SubscriptBox["y", 
           RowBox[{"a", ",", "1"}]], "]"}]}], "]"}], "\[IndentingNewLine]", 
       SubscriptBox["Dy", 
        RowBox[{"a", ",", "2"}]]}], "=", 
      RowBox[{
       RowBox[{"Simplify", "[", 
        RowBox[{
         SubscriptBox["\[PartialD]", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
         RowBox[{"Flatten", "[", 
          SubscriptBox["y", 
           RowBox[{"a", ",", "2"}]], "]"}]}], "]"}], "\n", 
       RowBox[{
        SubscriptBox["DLfy", 
         RowBox[{"a", ",", "1"}]], "=", 
        RowBox[{"Simplify", "[", 
         RowBox[{
          SubscriptBox["\[PartialD]", 
           RowBox[{"{", 
            RowBox[{
             RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
          RowBox[{"Flatten", "[", 
           RowBox[{
            SubscriptBox["Dy", 
             RowBox[{"a", ",", "1"}]], ".", "d\[Chi]"}], "]"}]}], 
         "]"}]}]}]}]}]}], ";", "\[IndentingNewLine]", 
   RowBox[{
    SubscriptBox["DLfy", 
     RowBox[{"a", ",", "2"}]], "=", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"Flatten", "[", 
       RowBox[{
        SubscriptBox["Dy", 
         RowBox[{"a", ",", "2"}]], ".", "d\[Chi]"}], "]"}]}], "]"}]}], ";"}], 
  "*)"}]], "Input",
 CellChangeTimes->{{3.513532586333204*^9, 3.513532677723161*^9}, {
   3.513532815648106*^9, 3.51353282124899*^9}, {3.513532861207259*^9, 
   3.513532869009141*^9}, {3.513532899901752*^9, 3.513532900419178*^9}, {
   3.51353311611145*^9, 3.513533116605757*^9}, 3.515521088606793*^9, {
   3.5155211643341246`*^9, 3.5155211667002597`*^9}, {3.5158526171702003`*^9, 
   3.515852621396442*^9}, {3.5158548842498703`*^9, 3.5158548875960617`*^9}, {
   3.5158551932005405`*^9, 3.5158551957126846`*^9}, {3.5158593451740203`*^9, 
   3.515859367792314*^9}, 3.51586024539751*^9, {3.5158611465420523`*^9, 
   3.515861146589055*^9}, 3.5158681409047847`*^9, {3.515869758310295*^9, 
   3.5158697588093233`*^9}, {3.515869821362901*^9, 3.515869822510967*^9}, {
   3.5158725172250957`*^9, 3.515872573737328*^9}, 3.5158863481049786`*^9, {
   3.515886571837773*^9, 3.5158865734445753`*^9}, {3.51595325150783*^9, 
   3.5159532575301747`*^9}, {3.5167083309077144`*^9, 
   3.5167083312677355`*^9}, {3.5167110381765614`*^9, 3.5167110528944035`*^9}, 
   3.5167111385673037`*^9, {3.5201093866961327`*^9, 3.520109388523237*^9}, 
   3.5201094274054613`*^9, 3.5201773866844225`*^9, {3.520437239317703*^9, 
   3.5204372535225153`*^9}, {3.5204490557876596`*^9, 
   3.5204490607579436`*^9}, {3.5207159029790354`*^9, 3.520715937276997*^9}, {
   3.527197199055368*^9, 3.527197204638042*^9}}],

Cell[BoxData[""], "Input"],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"Zero", " ", "Dynamics"}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"SetDirectory", "[", 
     RowBox[{
      RowBox[{"NotebookDirectory", "[", "]"}], "<>", 
      "\"\<buildopt_torso\>\""}], "]"}], ";", "\n", "\[IndentingNewLine]", 
    RowBox[{"Zstatesubs", " ", "=", " ", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["z", "1"], "[", "t", "]"}], "\[Rule]", " ", 
        FormBox[
         RowBox[{"z", "[", "1", "]"}],
         TraditionalForm]}], ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["z", "2"], "[", "t", "]"}], "\[Rule]", " ", 
        FormBox[
         RowBox[{"z", "[", "2", "]"}],
         TraditionalForm]}]}], "}"}]}], ";", "\n", 
    RowBox[{
     SubscriptBox["gamma", "0"], "=", " ", 
     RowBox[{"Simplify", "[", 
      RowBox[{"\[ScriptCapitalD]", "[", 
       RowBox[{"[", 
        RowBox[{"1", ",", ";;"}], "]"}], "]"}], "]"}]}], ";", 
    "\[IndentingNewLine]", 
    RowBox[{
     SubscriptBox["gamma", "f"], " ", "=", " ", 
     RowBox[{"First", "[", 
      RowBox[{"(", 
       RowBox[{"Simplify", "[", 
        RowBox[{
         SubscriptBox["gamma", "0"], ".", "dq"}], "]"}], ")"}], "]"}]}], ";", 
    "\n", 
    RowBox[{"Dgamma", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["gamma", "f"], " ", "]"}]}], "]"}]}], ";", "\n", 
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<gamma_sca\>\"", "]"}]}], ";", "\n", 
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       SubscriptBox["gamma", "f"], "/.", "statesubs"}]}], "]"}], ";", "\n", 
    RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<gamma0_sca\>\"", "]"}]}], ";", "\n", 
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       SubscriptBox["gamma", "0"], "/.", "statesubs"}]}], "]"}], ";", "\n", 
    RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<Dgamma_mat\>\"", "]"}]}], ";", "\n", 
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"Dgamma", "/.", "statesubs"}]}], "]"}], ";", "\n", 
    RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"testgamma", " ", "=", " ", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"(", 
        RowBox[{"D", "[", 
         RowBox[{
          RowBox[{
           RowBox[{"1", "/", "2"}], "*", 
           RowBox[{
            RowBox[{"Transpose", "[", "dq", "]"}], ".", "\[ScriptCapitalD]", 
            ".", "dq"}]}], ",", 
          FormBox[
           RowBox[{"dq", "[", 
            RowBox[{"[", 
             RowBox[{"1", ",", "1"}], "]"}], "]"}],
           TraditionalForm]}], "]"}], ")"}], "[", 
       RowBox[{"[", 
        RowBox[{"1", ",", "1"}], "]"}], "]"}], "]"}]}], ";", 
    "\[IndentingNewLine]", 
    RowBox[{
     RowBox[{"TrueQ", "[", 
      RowBox[{"testgamma", "\[Equal]", " ", 
       SubscriptBox["gamma", "f"]}], "]"}], "\n", "True", "\n", 
     RowBox[{"c", " ", "=", " ", 
      RowBox[{"Simplify", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[PartialD]", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
         RowBox[{"Flatten", "[", 
          SubscriptBox["deltap", "hip"], "]"}]}], "/.", "\[InvisibleSpace]", 
        "constsubs"}], "]"}]}]}], ";", "\[IndentingNewLine]", 
    RowBox[{"H", " ", "=", " ", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[PartialD]", 
         RowBox[{"{", 
          RowBox[{
           RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
        RowBox[{"Flatten", "[", 
         SubscriptBox["y", 
          RowBox[{"a", ",", "2"}]], "]"}]}], "/.", "\[InvisibleSpace]", 
       "constsubs"}], "]"}]}], ";", "\[IndentingNewLine]", 
    RowBox[{"Phi", " ", "=", " ", 
     RowBox[{"Join", "[", 
      RowBox[{
       RowBox[{"{", "c", "}"}], ",", "H"}], "]"}]}], ";", "\n", 
    RowBox[{"(*", 
     RowBox[{
      RowBox[{"fbz", "=", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"\[Sigma]", "[", "t", "]"}], "\[Rule]", 
         FractionBox[
          RowBox[{
           RowBox[{
            SubscriptBox["z", "1"], "[", "t", "]"}], "-", 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "2"}], "]"}]}], 
          RowBox[{"a", "[", 
           RowBox[{"1", ",", "1"}], "]"}]]}], "}"}]}], ";"}], "*)"}], 
    "\[IndentingNewLine]", 
    RowBox[{"fbz", "=", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"\[Sigma]", "[", "t", "]"}], "\[Rule]", 
       FractionBox[
        RowBox[{
         RowBox[{
          SubscriptBox["z", "1"], "[", "t", "]"}], "-", 
         RowBox[{"p", "[", "1", "]"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]}], "}"}]}], ";", "\n", 
    RowBox[{
     SubscriptBox["yz", 
      RowBox[{"d", ",", "2"}]], " ", "=", " ", 
     RowBox[{
      RowBox[{"Transpose", "[", 
       RowBox[{"{", 
        RowBox[{"Table", "[", 
         RowBox[{
          RowBox[{"HumanFunction", "[", "i", "]"}], ",", 
          RowBox[{"{", 
           RowBox[{"i", ",", " ", "2", ",", "ndof"}], "}"}]}], "]"}], "}"}], 
       "]"}], "/.", "fbz"}]}], ";", "\n", 
    RowBox[{"PhiI", "=", " ", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"Inverse", "[", "Phi", "]"}], ".", 
       RowBox[{"Join", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{
            SubscriptBox["z", "1"], "[", "t", "]"}], "}"}], "}"}], ",", 
         SubscriptBox["yz", 
          RowBox[{"d", ",", "2"}]]}], " ", "]"}]}], "]"}]}], ";", "\n", 
    RowBox[{"PhidotI", " ", "=", " ", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"Inverse", "[", "Phi", "]"}], ".", 
       RowBox[{"Join", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{
            SubscriptBox["z", "2"], "[", "t", "]"}], "}"}], "}"}], ",", 
         RowBox[{
          RowBox[{"D", "[", 
           RowBox[{
            SubscriptBox["yz", 
             RowBox[{"d", ",", "2"}]], ",", 
            RowBox[{
             SubscriptBox["z", "1"], "[", "t", "]"}]}], "]"}], "*", 
          RowBox[{
           SubscriptBox["z", "2"], "[", "t", "]"}]}]}], "]"}]}], "]"}]}], ";",
     "\n", 
    RowBox[{"zcond", " ", "=", " ", 
     RowBox[{"Table", "[", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"PhiI", "[", 
         RowBox[{"[", 
          RowBox[{"i", ",", "1"}], "]"}], "]"}]}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}]}], ";", "\n", 
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<Phi_inverse_mat\>\"", "]"}]}], ";", "\n", 
    
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"PhiI", "/.", "Zstatesubs"}]}], "]"}], ";", "\n", 
    RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<Phi_dot_inverse_mat\>\"", "]"}]}], ";", 
    "\n", 
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"PhidotI", "/.", "Zstatesubs"}]}], "]"}], ";", "\n", 
    RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
    SubscriptBox["gamma", "0"], ";", "\n", 
    RowBox[{"Psi", " ", "=", " ", 
     RowBox[{
      RowBox[{"Simplify", "[", 
       RowBox[{"Join", "[", 
        RowBox[{
         RowBox[{"{", 
          SubscriptBox["gamma", "0"], "}"}], ",", 
         RowBox[{"H", "-", 
          RowBox[{
           RowBox[{"D", "[", 
            RowBox[{
             SubscriptBox["yz", 
              RowBox[{"d", ",", "2"}]], ",", 
             RowBox[{
              SubscriptBox["z", "1"], "[", "t", "]"}]}], "]"}], ".", 
           RowBox[{"{", "c", "}"}]}]}]}], "]"}], "]"}], "/.", "zcond"}]}], 
    ";", "\n", 
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<Psi_mat\>\"", "]"}]}], ";", "\n", 
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"Psi", "/.", "Zstatesubs"}]}], "]"}], ";", "\n", 
    RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
    RowBox[{
     SubscriptBox["kappa", "1"], "=", 
     RowBox[{"First", "[", 
      RowBox[{"c", ".", "dq"}], "]"}]}], ";", "\n", 
    RowBox[{
     SubscriptBox["kappa", "2"], "=", 
     RowBox[{"-", 
      RowBox[{"Simplify", "[", 
       RowBox[{"D", "[", 
        RowBox[{"V", ",", 
         FormBox[
          RowBox[{"q", "[", 
           RowBox[{"[", 
            RowBox[{"1", ",", "1"}], "]"}], "]"}],
          TraditionalForm]}], "]"}], "]"}]}]}], ";", "\n", 
    RowBox[{
     SubscriptBox["f", "reduced"], " ", "=", " ", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"{", 
        SubscriptBox["kappa", "1"], "}"}], ",", 
       RowBox[{"{", 
        SubscriptBox["kappa", "2"], "}"}]}], "}"}]}], ";", "\n", 
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<f_reduced_mat\>\"", "]"}]}], ";", "\n", 
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       SubscriptBox["f", "reduced"], " ", "/.", "statesubs"}]}], "]"}], ";", 
    "\n", 
    RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"z1", " ", "=", " ", 
     RowBox[{
      RowBox[{
       RowBox[{"First", "[", 
        RowBox[{"c", ".", "q"}], "]"}], "\[IndentingNewLine]", "z2"}], " ", 
      "=", " ", 
      RowBox[{
       SubscriptBox["gamma", "f"], "\[IndentingNewLine]", 
       RowBox[{"zcomp", " ", "=", " ", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"{", "z1", "}"}], ",", 
          RowBox[{"{", "z2", "}"}]}], "}"}]}]}]}]}], ";", "\n", 
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<z_comp_mat\>\"", "]"}]}], ";", "\n", 
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"zcomp", " ", "/.", "statesubs"}]}], "]"}], ";", "\n", 
    RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"z1calc", " ", "=", " ", 
     RowBox[{"First", "[", 
      RowBox[{"c", ".", "q"}], "]"}]}], ";", "\[IndentingNewLine]", 
    RowBox[{"z2calc", " ", "=", " ", 
     RowBox[{"First", "[", 
      RowBox[{"c", ".", "dq"}], "]"}]}], ";", "\[IndentingNewLine]", 
    RowBox[{"zcalc", " ", "=", " ", 
     RowBox[{
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", "z1calc", "}"}], ",", 
         RowBox[{"{", "z2calc", "}"}]}], "}"}], "\n", "stream"}], "=", 
      RowBox[{"OpenWrite", "[", "\"\<z_calc_mat\>\"", "]"}]}]}], ";", "\n", 
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"zcalc", " ", "/.", "statesubs"}]}], "]"}], ";", "\n", 
    RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Dc", "=", 
     RowBox[{
      RowBox[{
       RowBox[{"Simplify", "[", 
        RowBox[{
         SubscriptBox["\[PartialD]", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
         RowBox[{"Flatten", "[", 
          RowBox[{"c", ".", "q"}], "]"}]}], "]"}], "\[IndentingNewLine]", 
       "DLfc"}], "=", 
      RowBox[{
       RowBox[{
        RowBox[{"Simplify", "[", 
         RowBox[{
          SubscriptBox["\[PartialD]", 
           RowBox[{"{", 
            RowBox[{
             RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
          RowBox[{"Flatten", "[", 
           RowBox[{"Dc", ".", "d\[Chi]"}], "]"}]}], "]"}], "\n", "stream"}], 
       "=", 
       RowBox[{"OpenWrite", "[", "\"\<DLfc_mat\>\"", "]"}]}]}]}], ";", "\n", 
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"DLfc", "/.", "statesubs"}]}], "]"}], ";", "\n", 
    RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"h", "/.", "zcond"}], ";", "\n", 
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<Psi_mat\>\"", "]"}]}], ";", "\n", 
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"Psi", "/.", "Zstatesubs"}]}], "]"}], ";", "\n", 
    RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"SetDirectory", "[", 
     RowBox[{"NotebookDirectory", "[", "]"}], "]"}], ";", 
    "\[IndentingNewLine]", 
    RowBox[{"Run", "[", "\"\<perl math2matopt_torso.pl\>\"", "]"}], ";", 
    "\[IndentingNewLine]", 
    RowBox[{"SetDirectory", "[", 
     RowBox[{
      RowBox[{"NotebookDirectory", "[", "]"}], "<>", 
      "\"\<buildopt_torso\>\""}], "]"}], ";"}], "*)"}]}]], "Input",
 CellChangeTimes->{{3.527197766090517*^9, 3.5271977735702133`*^9}, {
  3.527198064183445*^9, 3.527198075831641*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"Inverse", " ", "Kinematics"}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{"SetDirectory", "[", 
    RowBox[{
    "\"\</home/shu/workspace/Research/AMBER_Output_Test/model/\>\"", "<>", 
     "\"\<build_torso\>\""}], "]"}], ";"}]}]], "Input",
 CellChangeTimes->{{3.5158974201226616`*^9, 3.515897422208781*^9}, 
   3.5159409950429645`*^9, {3.5271981878879642`*^9, 3.52719819147633*^9}, {
   3.527198231717587*^9, 3.5271982390018187`*^9}}],

Cell["define the reset map", "Text",
 CellChangeTimes->{{3.514054915065277*^9, 3.514054918068027*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"\[CapitalDelta]", "=", " ", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{"1", ",", "1", ",", "1", ",", 
       RowBox[{"-", "1"}], ",", 
       RowBox[{"-", "1"}]}], "}"}], ",", "\[IndentingNewLine]", 
     RowBox[{"{", 
      RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "1"}], "}"}], ",", 
     "\[IndentingNewLine]", 
     RowBox[{"{", 
      RowBox[{"0", ",", "0", ",", "0", ",", "1", ",", "0"}], "}"}], ",", 
     "\[IndentingNewLine]", 
     RowBox[{"{", 
      RowBox[{"0", ",", "0", ",", "1", ",", "0", ",", "0"}], "}"}], ",", 
     "\[IndentingNewLine]", 
     RowBox[{"{", 
      RowBox[{"0", ",", "1", ",", "0", ",", "0", ",", "0"}], "}"}]}], "}"}]}],
   ";"}]], "Input",
 CellChangeTimes->{{3.5126870727453375`*^9, 3.5126871090532956`*^9}, {
   3.512687198656415*^9, 3.5126872022367573`*^9}, {3.5126906830481005`*^9, 
   3.5126907051903095`*^9}, {3.514054842423344*^9, 3.514054877019777*^9}, 
   3.514054909726557*^9, {3.5158588592522273`*^9, 3.5158589038437777`*^9}, {
   3.5158839903865247`*^9, 3.5158839971019087`*^9}, 3.515940988613597*^9, {
   3.515941980195312*^9, 3.515942020051592*^9}}],

Cell["define the guard conditions for pre- and post-impact", "Text",
 CellChangeTimes->{{3.5133462255725746`*^9, 3.5133462845249467`*^9}, {
  3.514049805517045*^9, 3.5140498067311144`*^9}, {3.514055491715686*^9, 
  3.514055499729824*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"initialtime", "=", 
   RowBox[{"{", 
    RowBox[{"t", "\[Rule]", "0"}], "}"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5158840455366793`*^9, 3.515884055622256*^9}, {
  3.5158841551359477`*^9, 3.5158841624563665`*^9}, {3.515891350156479*^9, 
  3.515891350585504*^9}}],

Cell["compute the position of the hip post-impact", "Text",
 CellChangeTimes->{{3.514055461182718*^9, 3.514055471246761*^9}, {
  3.514057296246017*^9, 3.514057317686638*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"resetmap", "=", 
  RowBox[{"Module", "[", 
   RowBox[{
    RowBox[{"{", "newq", "}"}], ",", "\[IndentingNewLine]", 
    RowBox[{
     RowBox[{"newq", "=", 
      RowBox[{"\[CapitalDelta]", ".", 
       RowBox[{"(", "q", ")"}]}]}], ";", "\[IndentingNewLine]", 
     RowBox[{"Table", "[", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "i"], "[", "0", "]"}], "->", 
        RowBox[{"newq", "[", 
         RowBox[{"[", 
          RowBox[{"i", ",", "1"}], "]"}], "]"}]}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "1", ",", 
         RowBox[{"Length", "[", "q", "]"}]}], "}"}]}], "]"}]}]}], 
   "\[IndentingNewLine]", "]"}]}]], "Input",
 CellChangeTimes->{
  3.515889034698043*^9, {3.5158896543664856`*^9, 3.515889655282538*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "\[Rule]", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "[", "0", "]"}], "\[Rule]", 
    RowBox[{
     SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "3"], "[", "0", "]"}], "\[Rule]", 
    RowBox[{
     SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "4"], "[", "0", "]"}], "\[Rule]", 
    RowBox[{
     SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "5"], "[", "0", "]"}], "\[Rule]", 
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]}], "}"}]], "Output",
 CellChangeTimes->{
  3.5158896559905787`*^9, 3.515891312993354*^9, 3.5158913534266663`*^9, 
   3.515894251635434*^9, 3.5158942862024117`*^9, 3.515894326961743*^9, 
   3.515894727589658*^9, 3.5158951402502604`*^9, {3.5158951959184446`*^9, 
   3.5158952146955185`*^9}, {3.5158958851218643`*^9, 3.515895898378623*^9}, 
   3.5158967997871804`*^9, 3.5158976253243985`*^9, 3.515941099709951*^9, 
   3.515941360918891*^9, 3.515942032578308*^9, 3.5159427206966662`*^9, {
   3.515942803684413*^9, 3.5159428203423653`*^9}, 3.51594611064756*^9, 
   3.515972658638178*^9, 3.5159857720962243`*^9, 3.516707085509482*^9, 
   3.5167098030309153`*^9, 3.516711289967963*^9, 3.5167126255793557`*^9, 
   3.516731910470809*^9, 3.5167515643706264`*^9, 3.5176792332544694`*^9, 
   3.517679752981196*^9, 3.5176800876443377`*^9, 3.5176859975213633`*^9, 
   3.5176980467735415`*^9, 3.51769848436257*^9, 3.5176985979950695`*^9, 
   3.520728267121224*^9, 3.5207285463751965`*^9, 3.5208973235081024`*^9, 
   3.52130401974968*^9, 3.5214860662895436`*^9, 3.521487934091376*^9, 
   3.52224513228377*^9, 3.5222468175321608`*^9, 3.5222499213874693`*^9, 
   3.5223307381127167`*^9, 3.522356630079946*^9, 3.522356934777317*^9, 
   3.5224296757648373`*^9, 3.5225104836975546`*^9, 3.522511209772084*^9, 
   3.5225118985574803`*^9, 3.522708163285374*^9, 3.5227751717417707`*^9, 
   3.522776865876011*^9, 3.5230286087460876`*^9, 3.5230313578373265`*^9, 
   3.5235572887965183`*^9, 3.5237248901425605`*^9, 3.5237265814943004`*^9, 
   3.523983761149135*^9, 3.5239850083154693`*^9, 3.523989323250269*^9, 
   3.5239903472678394`*^9, 3.5239920405716915`*^9, 3.5239921525100937`*^9, 
   3.5239948340604696`*^9, 3.5239949439737563`*^9, 3.5239950551251135`*^9, 
   3.5242481571317205`*^9, 3.5242485440988536`*^9, 3.524250023281294*^9, 
   3.524400154233082*^9, 3.524400327199837*^9, 3.5244063041029854`*^9, 
   3.5244065748633633`*^9, 3.5244068040493307`*^9, 3.524418417138015*^9, 
   3.5244187250733337`*^9, 3.524422248811038*^9, 3.5244223360330267`*^9, 
   3.524422776608226*^9, 3.5244238667405777`*^9, 3.5244422411595345`*^9, 
   3.524442766919606*^9, 3.5244438740479307`*^9, 3.5244457116200333`*^9, 
   3.524445889284196*^9, 3.5244469025996923`*^9, 3.5245024555219383`*^9, 
   3.5245119747777634`*^9, 3.524588415258215*^9, 3.5250903031414847`*^9, 
   3.5250917110573883`*^9, 3.525091967846143*^9, 3.525092206356729*^9, 
   3.5250924617768726`*^9, 3.525092581818059*^9, 3.5250927057322297`*^9, 
   3.5250929096535635`*^9, 3.5250932285091763`*^9, 3.525098788102166*^9, 
   3.5254480096751223`*^9, 3.5254612322982435`*^9, 3.525518290128872*^9, 
   3.5258245489251366`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell["Parameterized Time Using the Output of the Hip", "Subsection",
 CellChangeTimes->{{3.5133462255725746`*^9, 3.513346244405652*^9}, {
  3.51334632129305*^9, 3.513346341733219*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{"phipcondition", "=", 
  RowBox[{
   RowBox[{
    RowBox[{"{", 
     RowBox[{
      RowBox[{"p", "[", "1", "]"}], "\[Rule]", 
      SubscriptBox["deltap", "hip"]}], "}"}], "/.", "constsubs"}], "/.", 
   RowBox[{"t", "\[Rule]", "0"}]}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"phipcondition", "=", 
    RowBox[{
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "2"}], "]"}], "\[Rule]", 
        SubscriptBox["deltap", "hip"]}], "}"}], "/.", "constsubs"}], "/.", 
     RowBox[{"t", "\[Rule]", "0"}]}]}], "*)"}]}]}], "Input",
 CellChangeTimes->{{3.515889672571527*^9, 3.515889674968664*^9}, {
   3.515890444251664*^9, 3.515890444569683*^9}, {3.515942148250924*^9, 
   3.515942174667435*^9}, {3.5159426119234447`*^9, 3.5159426332196627`*^9}, 
   3.515943242212495*^9, 3.515943756731924*^9, {3.5159855129044*^9, 
   3.5159855341526146`*^9}, {3.5207282787238874`*^9, 
   3.5207282828571243`*^9}, {3.5223565377914543`*^9, 
   3.5223565440782657`*^9}, {3.5224294884521236`*^9, 3.5224294923883486`*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"p", "[", "1", "]"}], "\[Rule]", 
   RowBox[{"-", 
    FractionBox[
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"685", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "]"}]}], "+", 
        RowBox[{"514", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "0", "]"}]}], "]"}]}]}], 
       ")"}]}], "250000"]}]}], "}"}]], "Output",
 CellChangeTimes->{
  3.5158892342924585`*^9, {3.5158896677502513`*^9, 3.5158896815790424`*^9}, 
   3.5158904450737114`*^9, 3.5158913168935766`*^9, 3.515891355118763*^9, 
   3.515894251663436*^9, 3.5158942862234125`*^9, 3.5158943269927444`*^9, 
   3.515894727617659*^9, 3.5158951402732615`*^9, {3.5158951959444456`*^9, 
   3.5158952147145195`*^9}, {3.5158958851388655`*^9, 3.5158958984006243`*^9}, 
   3.515896799816182*^9, 3.5158976253443995`*^9, 3.5159411065893445`*^9, 
   3.515941363390033*^9, 3.515942035144455*^9, 3.515942176708552*^9, {
   3.515942621377986*^9, 3.5159426337216916`*^9}, {3.515942707017884*^9, 
   3.5159427366405783`*^9}, 3.5159428217274446`*^9, 3.5159432426405196`*^9, 
   3.515943757306957*^9, 3.5159461106765623`*^9, 3.51597265868318*^9, 
   3.5159857721222258`*^9, 3.5167070855404835`*^9, 3.5167098031069193`*^9, 
   3.516711290038967*^9, 3.5167126256843615`*^9, 3.5167319105188117`*^9, 
   3.5167515644066286`*^9, 3.5176792336174903`*^9, 3.5176797530431995`*^9, 
   3.517680087718342*^9, 3.517685997596368*^9, 3.517698046827544*^9, 
   3.517698484413573*^9, 3.5176985980660734`*^9, 3.520728283507161*^9, 
   3.5207285463971977`*^9, 3.5208973235321035`*^9, 3.521304019769681*^9, 
   3.521486066339546*^9, 3.521487934123378*^9, 3.522245132312772*^9, 
   3.522246817553162*^9, 3.522249921431472*^9, 3.5223307381467185`*^9, 
   3.5223566301735463`*^9, 3.5223569348085175`*^9, 3.5224296757768383`*^9, 
   3.522510483726557*^9, 3.522511209826087*^9, 3.5225118986154833`*^9, 
   3.522708163317376*^9, 3.5227751717677727`*^9, 3.5227768659130125`*^9, 
   3.5230286088150916`*^9, 3.523031357862328*^9, 3.5235572888235197`*^9, 
   3.5237248901755624`*^9, 3.5237265815333023`*^9, 3.5239837611731367`*^9, 
   3.523985008354471*^9, 3.523989323277271*^9, 3.5239903472948413`*^9, 
   3.523992040610693*^9, 3.523992152552096*^9, 3.5239948341104727`*^9, 
   3.5239949440117583`*^9, 3.523995055164116*^9, 3.524248157171723*^9, 
   3.5242485441318555`*^9, 3.5242500233182964`*^9, 3.5244001542390823`*^9, 
   3.5244003272168384`*^9, 3.524406304148988*^9, 3.5244065749033656`*^9, 
   3.5244068040813327`*^9, 3.524418417178015*^9, 3.524418725102335*^9, 
   3.5244222488550406`*^9, 3.524422336079029*^9, 3.5244227766152267`*^9, 
   3.5244238667775803`*^9, 3.5244422411785355`*^9, 3.5244427669546084`*^9, 
   3.524443874074932*^9, 3.5244457116500354`*^9, 3.5244458893091965`*^9, 
   3.5244469026336937`*^9, 3.52450245555694*^9, 3.5245119748137655`*^9, 
   3.524588415294217*^9, 3.525090303180487*^9, 3.5250917110953903`*^9, 
   3.5250919678861446`*^9, 3.525092206388731*^9, 3.5250924618118744`*^9, 
   3.5250925818710623`*^9, 3.5250927057752323`*^9, 3.525092909673564*^9, 
   3.525093228558179*^9, 3.525098788126168*^9, 3.525448009704124*^9, 
   3.5254612323242445`*^9, 3.525518290159874*^9, 3.525824548948138*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData["resetmap"], "Input"],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "\[Rule]", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "[", "0", "]"}], "\[Rule]", 
    RowBox[{
     SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "3"], "[", "0", "]"}], "\[Rule]", 
    RowBox[{
     SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "4"], "[", "0", "]"}], "\[Rule]", 
    RowBox[{
     SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], ",", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "5"], "[", "0", "]"}], "\[Rule]", 
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]}], "}"}]], "Output",
 CellChangeTimes->CompressedData["
1:eJwV0l9Ik2EYBfCNLEaWbWV5sbkK8bONhmFmMJ32fm5mrW26aK6xJCQrS7eG
o/qmoLFSqNTQQWvJulmboFc6LdYCK1t/pFqRsXDRzXD1qhCB4Iyo57k4/C4P
HM7uJruxmc/j8fQQh0C6NrqfEqGpIn7pACX9oQKRA8y5nVGiicBkvhNs7Er+
40CG7S9zgQu3ZtTobPvyfVJOieGILoqqBx2FHrB7LxexVFCi1B/zT4CPXs30
hsFE7CZhKimRGUpyi8C6HcXePeBSjyanDqzti41bwReN8wn0bD53qhtsKLz6
KQRafpUrRsDk8RNudKO8zSyooqTMxAll4GC6KoUaWUk8AMat0aIgmJKKV4KH
KDnfUTO7Csp3jvsz4PB2puQKoUT8eNN7VDHX8pMDl6wXWlyg/m2uqgOc+LhV
iyb4Gl8n6Jvmj6AD9oVJ1BkLaldBY+X6TvSLarE0A/Z8OOxmWdjhYtqLWlUb
rmvA00+/PUQ333E/R/9kGJsW7C0IeNBWt0SnA2lsnRkNx7cNoFtEljw9KDjK
CBvAa77v9ehXZXUSHYusPDODinvyBMrY5kwnsf/178tO8MeueQkHSiPU7gc/
M+muoWpKzmWnij3gvtG/BrTd9q4NbRU96UOnw9Fh9A2jHEO97qmXKKsbWkYX
824E7oJNzQdlrBr+lJXtrwVD4hrXGdBc+iBrDfwPmPUmgg==
  "]]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"phipcondition", "/.", "resetmap"}]], "Input",
 CellChangeTimes->{{3.516751800532134*^9, 3.516751805196401*^9}, {
  3.516752237310117*^9, 3.5167522378981504`*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"p", "[", "1", "]"}], "\[Rule]", 
   RowBox[{"-", 
    RowBox[{
     FractionBox["1", "250000"], 
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"514", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
        RowBox[{"685", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
       ")"}]}]}]}]}], "}"}]], "Output",
 CellChangeTimes->CompressedData["
1:eJwVyGtIk2EYgOElpYFjGNO5HxoUuM1Ek9xMptbeOWewdkhK17QDHcROrsMo
vzVScOUgYkGrRMYKWbbYjJpZIZNw0BCEIVH5JWNYM8y3jPqhfjPQnufHzQX3
thPWxtMbeDyeEVLEX64EKygR32dLQ2D78bqpiwpKngeLtlxCV/8p0U7h20Ib
6LIk1xmw2+SutIPRLzENmgxu95JqSuYqDRGU7bpc5AFNaWbUUkPJRMroGwbX
pLHeV2BN6DaR7KHkQXZFrhSUt5X3ycCUUyswgUH+RLgVtBxNsGj+XeZIN5ir
YT48Bf+Ea0sDoErQ3IN6VB3mzXsp2V1izykGX4jIHJp5bOuUH8xQjkkHwU5+
wdKgipJ+c8MkB75WDPvS4Oxjya5rhJL1LEEc7U2dXWDAcOO5M3YwwObVXgcT
00IdGpNp+x0gXckIoLZ38yOoWRTQcaBYnulAl87/kqdBB9fQo1ZTYvg934cO
2bKc9eDoZPIJ+vGmM4o+I7IOHeji/B50MVqo14Nx0SYzav8rdKNlqpZ8A6j/
JMlpBluFXw+gfLEmgQ65l8fNYFV9CYvOjHxuOgxOu9JXbaC3PVHA4Gd/Wn3g
obYfXffqKNnf8n2nB4yMrxnR2R3xC6isPHIHPTgw5kWTN6pDqFX65j1qiHsW
0W95t/wPwZPKqmK1hpIrC9m+feAMp7WfApe5RxtXwf/1wTVs
  "]]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"\[Sigma]", "[", "t", "]"}], "/.", "\[InvisibleSpace]", 
  "fb"}]], "Input"],

Cell[BoxData[
 FractionBox[
  RowBox[{
   RowBox[{"-", 
    RowBox[{"p", "[", "1", "]"}]}], "-", 
   FractionBox[
    RowBox[{"127", " ", 
     RowBox[{"(", 
      RowBox[{
       RowBox[{"685", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
       RowBox[{"514", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
      ")"}]}], "250000"]}], 
  RowBox[{"a", "[", 
   RowBox[{"1", ",", "1"}], "]"}]]], "Output",
 CellChangeTimes->CompressedData["
1:eJwVzF9IU2EYgPFVMr0IsxgK5Uwv2mmoSLqM3HR+a22HhseUcqdYf9AKLHWZ
Q93JqDCzi0XSZknY8GI6a6JSK2K0oEhDoiFFtQunY41oX+muos1o630vHn53
T1GzufHsBpFIxEHixKakp4KSIdf5xYt7KQnMMFs7Qb42VYUOtPqkFrBAGk5b
QZ1uqFIAq/IWtOjzU45RoqSk8Cf3Eo0Wd+1ygGt+wXdcRcmyssH5FHwrfTfo
BWuu24isBnRXSBjwyPCekd1g7yV99mEw27bwxAT6Q6EgqqDWE9dA1SHhoxs8
qVWXToLbB/l+lL1l5rPUlHyJCzlyUFWqiaKylsJFFziW8jMToCeW/3uilpJU
Nfs+Aa4ovc4kyJQw5T2Ekm/GLQF0NnYhZgWjZW2tAqiI51ZfBj99lhhQb73+
QR8Y0GdMorLM2DO0veORIQEuyzP70Ih9VZEEu4rYfo2GkubOHyNoypN14yDY
TVfGUXHLwBu0bZ+8wwBya+MOdOOOnXV1YAEn5lH1Nskd1NdkyuNA+2kmxwiG
GiMN6FJUu4ReMf15zYPTweIgavv3tekY6HCvd1vwPxvKt4Ln4r/MTtAyHLtq
P0BJmP1e5gB7w+l6NJIOtKO5Rv9t9GjPq1H07rxqCi2XvphD5yvvraK6vzdd
98EEs1+u0VIy83CzkwUfz+mFM2Diw1jGOvgf7eYv/w==
  "]]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"tau", "=", 
   RowBox[{"Simplify", "[", 
    RowBox[{
     RowBox[{
      RowBox[{
       RowBox[{
        RowBox[{"\[Sigma]", "[", "t", "]"}], "/.", "\[InvisibleSpace]", 
        "fb"}], "/.", "\[InvisibleSpace]", "phipcondition"}], "/.", 
      "resetmap"}], "/.", "constsubs"}], "]"}]}], 
  "\[IndentingNewLine]"}]], "Input",
 CellChangeTimes->{{3.5158895462843037`*^9, 3.5158895925569506`*^9}, {
  3.515889789857235*^9, 3.5158897931634245`*^9}, {3.515889831234602*^9, 
  3.5158898935841684`*^9}, {3.5158904699201326`*^9, 3.5158904714572206`*^9}, {
  3.515891377005015*^9, 3.515891377386037*^9}, {3.5159855481004124`*^9, 
  3.5159855596250715`*^9}}],

Cell[BoxData[
 RowBox[{
  FractionBox["1", 
   RowBox[{"250000", " ", 
    RowBox[{"a", "[", 
     RowBox[{"1", ",", "1"}], "]"}]}]], 
  RowBox[{"127", " ", 
   RowBox[{"(", 
    RowBox[{
     RowBox[{
      RowBox[{"-", "685"}], " ", 
      RowBox[{"Sin", "[", 
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "-", 
     RowBox[{"514", " ", 
      RowBox[{"Sin", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "+", 
     RowBox[{"514", " ", 
      RowBox[{"Sin", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
     RowBox[{"685", " ", 
      RowBox[{"Sin", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
    ")"}]}]}]], "Output",
 CellChangeTimes->{
  3.515889452554943*^9, {3.515889568025547*^9, 3.515889592954973*^9}, 
   3.515889692185649*^9, 3.5158898014698997`*^9, {3.51588983801899*^9, 
   3.5158898676326838`*^9}, 3.515889908068997*^9, {3.5158904521211147`*^9, 
   3.515890472167261*^9}, 3.5158913777660584`*^9, 3.515894251696438*^9, 
   3.5158942863154182`*^9, 3.51589432708775*^9, 3.5158947277126646`*^9, 
   3.515895140365267*^9, {3.515895196040451*^9, 3.5158952148045244`*^9}, {
   3.5158958852288704`*^9, 3.515895898487629*^9}, 3.5158967999501896`*^9, 
   3.5158976254364047`*^9, 3.5159411084254494`*^9, 3.515941365002125*^9, 
   3.515942036213516*^9, 3.515942715326359*^9, {3.515942799867194*^9, 
   3.515942823307535*^9}, 3.5159442992959566`*^9, 3.515946110855572*^9, 
   3.5159726588971925`*^9, 3.515985772270234*^9, 3.516707085705493*^9, 
   3.516709803409937*^9, 3.516711290475992*^9, 3.5167126260073805`*^9, 
   3.5167319108068285`*^9, 3.516751564688645*^9, 3.517679235595603*^9, 
   3.517679753524227*^9, 3.5176800882373714`*^9, 3.5176859981183977`*^9, 
   3.5176980472765703`*^9, 3.5176984847585926`*^9, 3.5176985985751023`*^9, 
   3.520728295358839*^9, 3.5207285464992037`*^9, 3.520897323632109*^9, 
   3.5213040198646865`*^9, 3.5214860665835605`*^9, 3.5214879342553854`*^9, 
   3.5222451324207783`*^9, 3.522246817640167*^9, 3.5222499215924807`*^9, 
   3.522330738269725*^9, 3.522356630345146*^9, 3.522356934980118*^9, 
   3.5224296758298407`*^9, 3.522510484010573*^9, 3.52251121023011*^9, 
   3.522511898821495*^9, 3.522708163525388*^9, 3.522775171867778*^9, 
   3.5227768661080236`*^9, 3.523028609083107*^9, 3.523031357965334*^9, 
   3.5235572889945292`*^9, 3.5237248902855687`*^9, 3.52372658166531*^9, 
   3.5239837612411404`*^9, 3.523985008517481*^9, 3.5239893234032784`*^9, 
   3.523990347406848*^9, 3.5239920407497015`*^9, 3.5239921526831036`*^9, 
   3.5239948342674813`*^9, 3.5239949441737676`*^9, 3.523995055323125*^9, 
   3.524248157332732*^9, 3.5242485442518625`*^9, 3.524250023466305*^9, 
   3.5244001542610836`*^9, 3.524400327281842*^9, 3.5244063043119974`*^9, 
   3.5244065750293727`*^9, 3.52440680419934*^9, 3.5244184173280153`*^9, 
   3.5244187252123413`*^9, 3.524422249012049*^9, 3.5244223362370386`*^9, 
   3.5244227766332273`*^9, 3.5244238669215884`*^9, 3.52444224124854*^9, 
   3.524442767109617*^9, 3.5244438741809382`*^9, 3.5244457117580414`*^9, 
   3.5244458893892016`*^9, 3.524446902790703*^9, 3.5245024558719587`*^9, 
   3.5245119749607735`*^9, 3.5245884154382253`*^9, 3.525090303333496*^9, 
   3.525091711232398*^9, 3.525091968031153*^9, 3.525092206491737*^9, 
   3.525092461953882*^9, 3.525092581984069*^9, 3.5250927059392414`*^9, 
   3.5250929097735634`*^9, 3.525093228739189*^9, 3.525098788242174*^9, 
   3.5254480098121305`*^9, 3.5254612324172497`*^9, 3.5255182902728806`*^9, 
   3.5258245492001524`*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<tau\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"tau", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.515891747597212*^9, 3.5158917482732506`*^9}}],

Cell["extract the (relative degree two) position-based outputs", "Text",
 CellChangeTimes->{{3.51405623270548*^9, 3.514056248417758*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  SubscriptBox["y", "2"], "=", " ", 
  RowBox[{
   RowBox[{
    RowBox[{"(", 
     RowBox[{
      SubscriptBox["y", 
       RowBox[{"a", ",", "2"}]], "  ", "-", " ", 
      SubscriptBox["y", 
       RowBox[{"d", ",", "2"}]]}], "  ", ")"}], "/.", "\[InvisibleSpace]", 
    "phipcondition"}], "/.", "constsubs"}]}]], "Input",
 CellChangeTimes->{{3.5159429875229278`*^9, 3.515943032690511*^9}, {
  3.51598557480694*^9, 3.5159855753639717`*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"2", ",", "5"}], "]"}]}], "-", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       RowBox[{"-", 
        FractionBox[
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"2", ",", "4"}], "]"}], " ", 
          RowBox[{"(", 
           RowBox[{
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"685", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "]"}]}], 
                "+", 
                RowBox[{"514", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "0", "]"}]}], 
                  "]"}]}]}], ")"}]}], "250000"], "-", 
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"685", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                "+", 
                RowBox[{"514", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                  "]"}]}]}], ")"}]}], "250000"]}], ")"}]}], 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]]}]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "0", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"], "-", 
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], ")"}]}]}], "]"}]}], "+", 
        
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "0", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"], "-", 
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], ")"}]}]}], "]"}]}]}], 
       ")"}]}], "+", 
     FractionBox[
      RowBox[{
       RowBox[{
        RowBox[{"-", "514"}], " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "-", 
       RowBox[{"685", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
      RowBox[{
       RowBox[{"514", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
       RowBox[{"685", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}]]}], 
    "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"3", ",", "5"}], "]"}]}], "-", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       RowBox[{"-", 
        FractionBox[
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"3", ",", "4"}], "]"}], " ", 
          RowBox[{"(", 
           RowBox[{
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"685", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "]"}]}], 
                "+", 
                RowBox[{"514", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "0", "]"}]}], 
                  "]"}]}]}], ")"}]}], "250000"], "-", 
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"685", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                "+", 
                RowBox[{"514", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                  "]"}]}]}], ")"}]}], "250000"]}], ")"}]}], 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]]}]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"3", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "0", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"], "-", 
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], ")"}]}]}], "]"}]}], "+", 
        
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"3", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "0", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"], "-", 
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], ")"}]}]}], "]"}]}]}], 
       ")"}]}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"4", ",", "5"}], "]"}]}], "-", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       RowBox[{"-", 
        FractionBox[
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"4", ",", "4"}], "]"}], " ", 
          RowBox[{"(", 
           RowBox[{
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"685", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "]"}]}], 
                "+", 
                RowBox[{"514", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "0", "]"}]}], 
                  "]"}]}]}], ")"}]}], "250000"], "-", 
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"685", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                "+", 
                RowBox[{"514", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                  "]"}]}]}], ")"}]}], "250000"]}], ")"}]}], 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]]}]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "0", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"], "-", 
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], ")"}]}]}], "]"}]}], "+", 
        
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "0", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"], "-", 
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], ")"}]}]}], "]"}]}]}], 
       ")"}]}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"5", ",", "5"}], "]"}]}], "-", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       RowBox[{"-", 
        FractionBox[
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"5", ",", "4"}], "]"}], " ", 
          RowBox[{"(", 
           RowBox[{
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"685", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "]"}]}], 
                "+", 
                RowBox[{"514", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "0", "]"}]}], 
                  "]"}]}]}], ")"}]}], "250000"], "-", 
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"685", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                "+", 
                RowBox[{"514", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                  "]"}]}]}], ")"}]}], "250000"]}], ")"}]}], 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]]}]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "0", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"], "-", 
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], ")"}]}]}], "]"}]}], "+", 
        
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "0", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "0", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"], "-", 
              FractionBox[
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                  "+", 
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], ")"}]}]}], "]"}]}]}], 
       ")"}]}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}]}], 
  "}"}]], "Output",
 CellChangeTimes->{
  3.515890267326545*^9, 3.5158905060692005`*^9, 3.51589425173344*^9, 
   3.51589428635242*^9, 3.5158943271307526`*^9, 3.5158947277496667`*^9, 
   3.515895140404269*^9, {3.5158951960774536`*^9, 3.5158952148375263`*^9}, {
   3.5158958852608724`*^9, 3.515895898523631*^9}, 3.5158967999951925`*^9, 
   3.5158976254704065`*^9, 3.5159411188630466`*^9, 3.5159413671962504`*^9, 
   3.515942038314636*^9, 3.515942857994519*^9, {3.5159429911471353`*^9, 
   3.5159430336865683`*^9}, 3.5159443036172037`*^9, 3.5159461108905745`*^9, 
   3.515964447936552*^9, 3.5159726589391947`*^9, 3.515985772310237*^9, 
   3.5167070857444954`*^9, 3.5167098034899416`*^9, 3.516711290560997*^9, 
   3.5167126260793843`*^9, 3.5167319108548307`*^9, 3.5167515647386475`*^9, 
   3.517679235654607*^9, 3.517679753595231*^9, 3.5176800884303827`*^9, 
   3.5176859982004023`*^9, 3.5176980473385735`*^9, 3.5176984848335967`*^9, 
   3.5176985986531067`*^9, 3.52072830236724*^9, 3.5207285465332055`*^9, 
   3.520897323675112*^9, 3.5213040199376907`*^9, 3.521486066642564*^9, 
   3.5214879343343897`*^9, 3.52224513245478*^9, 3.522246817670169*^9, 
   3.522249921646484*^9, 3.522330738307728*^9, 3.522356630391947*^9, 
   3.5223569350269175`*^9, 3.522429675851842*^9, 3.522510484052575*^9, 
   3.5225112102931137`*^9, 3.522511898881499*^9, 3.522708163583391*^9, 
   3.5227751718977795`*^9, 3.5227768661530266`*^9, 3.523028609172112*^9, 
   3.5230313580053363`*^9, 3.5235572890385313`*^9, 3.523724890321571*^9, 
   3.5237265817073126`*^9, 3.523983761264142*^9, 3.523985008560483*^9, 
   3.5239893234342794`*^9, 3.5239903474388494`*^9, 3.523992040788704*^9, 
   3.5239921527191057`*^9, 3.5239948343084836`*^9, 3.52399494421377*^9, 
   3.523995055371128*^9, 3.5242481573757343`*^9, 3.5242485442858644`*^9, 
   3.524250023524308*^9, 3.5244001542760844`*^9, 3.524400327302843*^9, 
   3.5244063043589997`*^9, 3.5244065750633745`*^9, 3.524406804230341*^9, 
   3.5244184173780155`*^9, 3.5244187252423434`*^9, 3.5244222490610523`*^9, 
   3.524422336284041*^9, 3.5244227766462283`*^9, 3.5244238669625907`*^9, 
   3.524442241270541*^9, 3.52444276715562*^9, 3.52444387421094*^9, 
   3.524445711788043*^9, 3.524445889412203*^9, 3.5244469028357058`*^9, 
   3.524502455914961*^9, 3.5245119749967756`*^9, 3.5245884154782276`*^9, 
   3.5250903033754983`*^9, 3.5250917112674*^9, 3.525091968072156*^9, 
   3.525092206523739*^9, 3.5250924619888844`*^9, 3.52509258201307*^9, 
   3.5250927059842443`*^9, 3.5250929098035636`*^9, 3.5250932288011923`*^9, 
   3.525098788275176*^9, 3.5254480098661337`*^9, 3.5254612324482517`*^9, 
   3.525518290383887*^9, 3.5258245492861576`*^9}]
}, Open  ]],

Cell["compute the pre- and post-impact values of the outputs", "Text",
 CellChangeTimes->{{3.514056343774789*^9, 3.514056355919777*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"yplus", "=", 
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["y", "2"], "/.", "initialtime"}], "/.", "resetmap"}], "//", 
   "Simplify"}]}]], "Input",
 CellChangeTimes->{{3.5159428874992065`*^9, 3.5159429143387423`*^9}, {
  3.51594304105899*^9, 3.5159430447622013`*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"2", ",", "1"}], "]"}]}], "-", 
     RowBox[{"a", "[", 
      RowBox[{"2", ",", "5"}], "]"}], "-", 
     FractionBox[
      RowBox[{
       RowBox[{"685", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
       RowBox[{"514", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
      RowBox[{
       RowBox[{"685", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
       RowBox[{"514", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}]]}], 
    "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"3", ",", "1"}], "]"}]}], "-", 
     RowBox[{"a", "[", 
      RowBox[{"3", ",", "5"}], "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"4", ",", "1"}], "]"}]}], "-", 
     RowBox[{"a", "[", 
      RowBox[{"4", ",", "5"}], "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"5", ",", "1"}], "]"}]}], "-", 
     RowBox[{"a", "[", 
      RowBox[{"5", ",", "5"}], "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}]}], 
  "}"}]], "Output",
 CellChangeTimes->{{3.5159428883502555`*^9, 3.515942914923775*^9}, 
   3.5159430466363087`*^9, 3.515943096152141*^9, 3.515944306763384*^9, 
   3.515946111471607*^9, 3.5159644514537535`*^9, 3.5159726595632305`*^9, 
   3.5159857729882755`*^9, 3.5167070864565363`*^9, 3.5167098040589743`*^9, 
   3.51671129078701*^9, 3.5167126263153977`*^9, 3.516731911083844*^9, 
   3.5167515649466596`*^9, 3.5176792358986206`*^9, 3.5176797538342447`*^9, 
   3.517680088689398*^9, 3.517685998558423*^9, 3.517698047567587*^9, 
   3.5176984850736103`*^9, 3.517698598901121*^9, 3.5207283066354837`*^9, 
   3.520728546560207*^9, 3.520897323711114*^9, 3.521304019983693*^9, 
   3.5214860666915665`*^9, 3.521487934370392*^9, 3.522245132486782*^9, 
   3.5222468176981707`*^9, 3.522249921700487*^9, 3.5223307383427296`*^9, 
   3.5223566304387465`*^9, 3.522356935073718*^9, 3.522429675866843*^9, 
   3.522510484249586*^9, 3.5225112106221323`*^9, 3.5225118989345016`*^9, 
   3.522708163625394*^9, 3.5227751719307814`*^9, 3.522776866190029*^9, 
   3.523028609257117*^9, 3.523031358036338*^9, 3.5235572890745335`*^9, 
   3.523724890356573*^9, 3.523726581748315*^9, 3.523983761289143*^9, 
   3.523985008605486*^9, 3.5239893234672813`*^9, 3.5239903474728518`*^9, 
   3.5239920408307056`*^9, 3.523992152759108*^9, 3.523994834351486*^9, 
   3.5239949442567725`*^9, 3.5239950554171305`*^9, 3.5242481574207373`*^9, 
   3.5242485443208666`*^9, 3.5242500235673103`*^9, 3.5244001542920856`*^9, 
   3.524400327328845*^9, 3.524406304406002*^9, 3.5244065750983763`*^9, 
   3.5244068042643433`*^9, 3.524418417438016*^9, 3.5244187252743454`*^9, 
   3.5244222491090546`*^9, 3.5244223363310432`*^9, 3.5244227766602287`*^9, 
   3.5244238670035934`*^9, 3.5244422412955427`*^9, 3.5244427671976223`*^9, 
   3.524443874243942*^9, 3.5244457118200455`*^9, 3.5244458894382043`*^9, 
   3.5244469028727074`*^9, 3.524502455952963*^9, 3.5245119750337777`*^9, 
   3.52458841552223*^9, 3.5250903034155006`*^9, 3.5250917113074026`*^9, 
   3.5250919681141577`*^9, 3.52509220655274*^9, 3.5250924620258865`*^9, 
   3.5250925820460725`*^9, 3.5250927060322466`*^9, 3.525092909833564*^9, 
   3.525093228860196*^9, 3.5250987883101788`*^9, 3.525448009899135*^9, 
   3.5254612324792533`*^9, 3.5255182904268894`*^9, 3.5258245494881687`*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{"\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<y_plus\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"yplus", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}]}]}]], "Input",
 CellChangeTimes->{{3.515893576706831*^9, 3.51589357756488*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"yminus", "=", 
  RowBox[{
   SubscriptBox["y", "2"], "/.", "resetmap"}]}]], "Input",
 CellChangeTimes->{{3.5158912072393045`*^9, 3.515891227677474*^9}, {
  3.515943128370984*^9, 3.515943134586339*^9}, {3.515943165683118*^9, 
  3.515943166187147*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"2", ",", "5"}], "]"}]}], "-", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       RowBox[{"-", 
        FractionBox[
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"2", ",", "4"}], "]"}], " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"-", 
             FractionBox[
              RowBox[{"127", " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"685", " ", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                 "+", 
                 RowBox[{"514", " ", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                   "]"}]}]}], ")"}]}], "250000"]}], "+", 
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"514", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                 "+", 
                RowBox[{"685", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                  "]"}]}]}], ")"}]}], "250000"]}], ")"}]}], 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]]}]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               FractionBox[
                RowBox[{"127", " ", 
                 RowBox[{"(", 
                  RowBox[{
                   RowBox[{"685", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                   "+", 
                   RowBox[{"514", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], "+", 
              RowBox[{
               RowBox[{"1", "/", "250000"}], 
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}]}]}], ")"}]}]}], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"2", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               FractionBox[
                RowBox[{"127", " ", 
                 RowBox[{"(", 
                  RowBox[{
                   RowBox[{"685", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                   "+", 
                   RowBox[{"514", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], "+", 
              RowBox[{
               RowBox[{"1", "/", "250000"}], 
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}]}]}], ")"}]}]}], "]"}]}]}], ")"}]}], 
     "+", 
     FractionBox[
      RowBox[{
       RowBox[{
        RowBox[{"-", "514"}], " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "-", 
       RowBox[{"685", " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
      RowBox[{
       RowBox[{"514", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
       RowBox[{"685", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}]]}], 
    "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"3", ",", "5"}], "]"}]}], "-", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       RowBox[{"-", 
        FractionBox[
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"3", ",", "4"}], "]"}], " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"-", 
             FractionBox[
              RowBox[{"127", " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"685", " ", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                 "+", 
                 RowBox[{"514", " ", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                   "]"}]}]}], ")"}]}], "250000"]}], "+", 
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"514", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                 "+", 
                RowBox[{"685", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                  "]"}]}]}], ")"}]}], "250000"]}], ")"}]}], 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]]}]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"3", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               FractionBox[
                RowBox[{"127", " ", 
                 RowBox[{"(", 
                  RowBox[{
                   RowBox[{"685", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                   "+", 
                   RowBox[{"514", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], "+", 
              RowBox[{
               RowBox[{"1", "/", "250000"}], 
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}]}]}], ")"}]}]}], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"3", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"3", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               FractionBox[
                RowBox[{"127", " ", 
                 RowBox[{"(", 
                  RowBox[{
                   RowBox[{"685", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                   "+", 
                   RowBox[{"514", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], "+", 
              RowBox[{
               RowBox[{"1", "/", "250000"}], 
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}]}]}], ")"}]}]}], "]"}]}]}], ")"}]}], 
     "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"4", ",", "5"}], "]"}]}], "-", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       RowBox[{"-", 
        FractionBox[
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"4", ",", "4"}], "]"}], " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"-", 
             FractionBox[
              RowBox[{"127", " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"685", " ", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                 "+", 
                 RowBox[{"514", " ", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                   "]"}]}]}], ")"}]}], "250000"]}], "+", 
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"514", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                 "+", 
                RowBox[{"685", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                  "]"}]}]}], ")"}]}], "250000"]}], ")"}]}], 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]]}]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               FractionBox[
                RowBox[{"127", " ", 
                 RowBox[{"(", 
                  RowBox[{
                   RowBox[{"685", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                   "+", 
                   RowBox[{"514", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], "+", 
              RowBox[{
               RowBox[{"1", "/", "250000"}], 
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}]}]}], ")"}]}]}], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"4", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               FractionBox[
                RowBox[{"127", " ", 
                 RowBox[{"(", 
                  RowBox[{
                   RowBox[{"685", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                   "+", 
                   RowBox[{"514", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], "+", 
              RowBox[{
               RowBox[{"1", "/", "250000"}], 
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}]}]}], ")"}]}]}], "]"}]}]}], ")"}]}], 
     "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", 
      RowBox[{"a", "[", 
       RowBox[{"5", ",", "5"}], "]"}]}], "-", 
     RowBox[{
      SuperscriptBox["\[ExponentialE]", 
       RowBox[{"-", 
        FractionBox[
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"5", ",", "4"}], "]"}], " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"-", 
             FractionBox[
              RowBox[{"127", " ", 
               RowBox[{"(", 
                RowBox[{
                 RowBox[{"685", " ", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                 "+", 
                 RowBox[{"514", " ", 
                  RowBox[{"Sin", "[", 
                   RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                   "]"}]}]}], ")"}]}], "250000"]}], "+", 
            FractionBox[
             RowBox[{"127", " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{"514", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                 "+", 
                RowBox[{"685", " ", 
                 RowBox[{"Sin", "[", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                   RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                  "]"}]}]}], ")"}]}], "250000"]}], ")"}]}], 
         RowBox[{"a", "[", 
          RowBox[{"1", ",", "1"}], "]"}]]}]], " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               FractionBox[
                RowBox[{"127", " ", 
                 RowBox[{"(", 
                  RowBox[{
                   RowBox[{"685", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                   "+", 
                   RowBox[{"514", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], "+", 
              RowBox[{
               RowBox[{"1", "/", "250000"}], 
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}]}]}], ")"}]}]}], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           FractionBox["1", 
            RowBox[{"a", "[", 
             RowBox[{"1", ",", "1"}], "]"}]], 
           RowBox[{
            RowBox[{"a", "[", 
             RowBox[{"5", ",", "2"}], "]"}], " ", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"-", 
               FractionBox[
                RowBox[{"127", " ", 
                 RowBox[{"(", 
                  RowBox[{
                   RowBox[{"685", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], 
                   "+", 
                   RowBox[{"514", " ", 
                    RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}], "250000"]}], "+", 
              RowBox[{
               RowBox[{"1", "/", "250000"}], 
               RowBox[{"127", " ", 
                RowBox[{"(", 
                 RowBox[{
                  RowBox[{"514", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}],
                   "+", 
                  RowBox[{"685", " ", 
                   RowBox[{"Sin", "[", 
                    RowBox[{
                    RowBox[{
                    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
                    RowBox[{
                    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], 
                    "]"}]}]}], ")"}]}]}]}], ")"}]}]}], "]"}]}]}], ")"}]}], 
     "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}]}], 
  "}"}]], "Output",
 CellChangeTimes->{{3.5158912127146177`*^9, 3.5158912281054983`*^9}, 
   3.5158942517994437`*^9, 3.5158942864094234`*^9, 3.515894327207757*^9, 
   3.51589472781367*^9, 3.515895140463272*^9, {3.5158951961434574`*^9, 
   3.5158952148955297`*^9}, {3.515895885310875*^9, 3.5158958986236367`*^9}, 
   3.5158968000671964`*^9, 3.51589762552641*^9, 3.5159411532770147`*^9, 
   3.515941372136533*^9, 3.515942044244975*^9, 3.5159431301160836`*^9, {
   3.515943162203919*^9, 3.5159431667031765`*^9}, 3.5159443146768365`*^9, 
   3.5159461115766134`*^9, 3.515964475797146*^9, 3.5159726596492357`*^9, 
   3.515985773047279*^9, 3.5167070865455413`*^9, 3.5167098042449846`*^9, 
   3.516711290858014*^9, 3.5167126264794073`*^9, 3.5167319111848497`*^9, 
   3.5167515649956627`*^9, 3.5176792364036493`*^9, 3.517679754050257*^9, 
   3.517680088841406*^9, 3.5176859987134314`*^9, 3.517698047746597*^9, 
   3.5176984853146243`*^9, 3.517698599124134*^9, 3.5207283137618914`*^9, 
   3.520728546596209*^9, 3.520897323754116*^9, 3.521304020053697*^9, 
   3.5214860667565703`*^9, 3.521487934416394*^9, 3.522245132521784*^9, 
   3.5222468177561736`*^9, 3.5222499217654905`*^9, 3.5223307383847322`*^9, 
   3.522356630485547*^9, 3.522356935136118*^9, 3.522429675887844*^9, 
   3.5225104843265905`*^9, 3.522511210681136*^9, 3.5225118990025053`*^9, 
   3.5227081636723967`*^9, 3.522775171962784*^9, 3.5227768662330313`*^9, 
   3.5230286093411217`*^9, 3.523031358107342*^9, 3.523557289116536*^9, 
   3.523724890394575*^9, 3.5237265817913175`*^9, 3.5239837613121443`*^9, 
   3.5239850086494884`*^9, 3.5239893235012836`*^9, 3.523990347505853*^9, 
   3.5239920408737087`*^9, 3.52399215280011*^9, 3.523994834396489*^9, 
   3.523994944306775*^9, 3.523995055463133*^9, 3.52424815746474*^9, 
   3.5242485443548684`*^9, 3.524250023671316*^9, 3.524400154307086*^9, 
   3.524400327349846*^9, 3.524406304455005*^9, 3.5244065751323786`*^9, 
   3.5244068042913437`*^9, 3.5244184174880157`*^9, 3.5244187253073473`*^9, 
   3.5244222491590576`*^9, 3.5244223363810463`*^9, 3.52442277667523*^9, 
   3.5244238670445957`*^9, 3.5244422413175435`*^9, 3.5244427672426248`*^9, 
   3.524443874273944*^9, 3.524445711852047*^9, 3.5244458894642057`*^9, 
   3.5244469029207106`*^9, 3.5245024559959655`*^9, 3.5245119750807805`*^9, 
   3.5245884155622325`*^9, 3.5250903034555025`*^9, 3.5250917113474045`*^9, 
   3.5250919681561604`*^9, 3.525092206583742*^9, 3.525092462064889*^9, 
   3.5250925820800743`*^9, 3.5250927060802493`*^9, 3.525092909873564*^9, 
   3.5250932289272*^9, 3.5250987883421803`*^9, 3.5254480099391375`*^9, 
   3.5254612325142555`*^9, 3.5255182904698915`*^9, 3.5258245495211706`*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<y_minus\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"yminus", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], 
  ";"}], "\[IndentingNewLine]"}], "Input",
 CellChangeTimes->{{3.5158935571417117`*^9, 3.515893586749405*^9}}],

Cell[CellGroupData[{

Cell["solve for the inverse kinematics", "Subsubsection",
 CellChangeTimes->{{3.5133462255725746`*^9, 3.5133462845249467`*^9}, {
  3.5140475842599964`*^9, 3.514047587214165*^9}, {3.514050381757004*^9, 
  3.5140503867632904`*^9}, {3.5140508768053193`*^9, 3.5140508825446477`*^9}, {
  3.514056179159212*^9, 3.51405618309467*^9}}],

Cell["create a function to solve the inverse kinematics", "Text",
 CellChangeTimes->{{3.514058861590675*^9, 3.514058871479416*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{
   RowBox[{"SolveEquations", "[", 
    RowBox[{"equations_", ",", "variables_", ",", "solutionnumbers_"}], "]"}],
    ":=", "\[IndentingNewLine]", 
   RowBox[{"Module", "[", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{"i", ",", 
       RowBox[{"solutions", "=", 
        RowBox[{"{", "}"}]}], ",", "tmpeqn", ",", "tmpsolution"}], "}"}], ",",
      "\[IndentingNewLine]", 
     RowBox[{
      RowBox[{"For", "[", 
       RowBox[{
        RowBox[{"i", "=", "1"}], ",", 
        RowBox[{"i", "\[LessEqual]", 
         RowBox[{"Length", "[", "variables", "]"}]}], ",", 
        RowBox[{"i", "++"}], ",", "\[IndentingNewLine]", 
        RowBox[{
         RowBox[{"tmpeqn", "=", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{"equations", "[", 
             RowBox[{"[", "i", "]"}], "]"}], ")"}], "\[Equal]", "0"}]}], ";", 
         "\[IndentingNewLine]", 
         RowBox[{"tmpsolution", "=", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{
            "variables", "\[LeftDoubleBracket]", "i", 
             "\[RightDoubleBracket]"}], "\[Rule]", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{
              "variables", "\[LeftDoubleBracket]", "i", 
               "\[RightDoubleBracket]"}], "/.", 
              RowBox[{"Flatten", "[", 
               RowBox[{"{", 
                RowBox[{"solutions", ",", 
                 RowBox[{
                  RowBox[{"Solve", "[", 
                   RowBox[{"tmpeqn", ",", 
                    RowBox[{
                    "variables", "\[LeftDoubleBracket]", "i", 
                    "\[RightDoubleBracket]"}]}], "]"}], 
                  "\[LeftDoubleBracket]", 
                  RowBox[{
                  "solutionnumbers", "\[LeftDoubleBracket]", "i", 
                   "\[RightDoubleBracket]"}], "\[RightDoubleBracket]"}]}], 
                "}"}], "]"}]}], ")"}]}], "}"}]}], ";", "\[IndentingNewLine]", 
         
         RowBox[{"solutions", "=", 
          RowBox[{"Join", "[", 
           RowBox[{"solutions", ",", 
            RowBox[{"(", 
             RowBox[{"tmpsolution", "/.", "solutions"}], ")"}]}], "]"}]}]}]}],
        "\[IndentingNewLine]", "]"}], ";", "\[IndentingNewLine]", 
      "solutions"}]}], "\[IndentingNewLine]", "]"}]}], ";"}]], "Input"],

Cell[BoxData[
 RowBox[{
  RowBox[{"atest", "=", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"1", ",", "1"}], "]"}], "\[Rule]", "1.19689396035823`"}], ",", 
     
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"1", ",", "2"}], "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"1", ",", "3"}], "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"1", ",", "4"}], "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"1", ",", "5"}], "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"2", ",", "1"}], "]"}], "\[Rule]", "0.20214668746590925`"}], 
     ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"2", ",", "2"}], "]"}], "\[Rule]", "7.840374588741148`"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"2", ",", "3"}], "]"}], "\[Rule]", "0.22483245436904015`"}], 
     ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"2", ",", "4"}], "]"}], "\[Rule]", 
      RowBox[{"-", "0.8069757285872418`"}]}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"2", ",", "5"}], "]"}], "\[Rule]", "0.18712786224886793`"}], 
     ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"3", ",", "1"}], "]"}], "\[Rule]", 
      RowBox[{"-", "0.0828259043763153`"}]}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"3", ",", "2"}], "]"}], "\[Rule]", "13.315981803707265`"}], 
     ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"3", ",", "3"}], "]"}], "\[Rule]", "0.20729365656915424`"}], 
     ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"3", ",", "4"}], "]"}], "\[Rule]", "4.154819403051306`"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"3", ",", "5"}], "]"}], "\[Rule]", "0.2573791697685864`"}], 
     ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"4", ",", "1"}], "]"}], "\[Rule]", 
      RowBox[{"-", "0.3808535485970228`"}]}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"4", ",", "2"}], "]"}], "\[Rule]", 
      RowBox[{"-", "10.979481912107921`"}]}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"4", ",", "3"}], "]"}], "\[Rule]", 
      RowBox[{"-", "0.19665480379455527`"}]}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"4", ",", "4"}], "]"}], "\[Rule]", 
      RowBox[{"-", "0.42141500655251124`"}]}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"4", ",", "5"}], "]"}], "\[Rule]", "0.6584527300961083`"}], 
     ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"5", ",", "1"}], "]"}], "\[Rule]", 
      RowBox[{"-", "3.261238467195124`"}]}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"5", ",", "2"}], "]"}], "\[Rule]", 
      RowBox[{"-", "2.0631556896285772`*^-8"}]}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"5", ",", "3"}], "]"}], "\[Rule]", "5.819659430757391`"}], ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"5", ",", "4"}], "]"}], "\[Rule]", "0.37746704753242283`"}], 
     ",", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"5", ",", "5"}], "]"}], "\[Rule]", "3.023429578158533`"}]}], 
    "}"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.516708534127338*^9, 3.516708546737059*^9}, {
  3.5167085869383583`*^9, 3.516708631586912*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"A", "=", 
   RowBox[{"Table", "[", 
    RowBox[{
     RowBox[{"a", "[", 
      RowBox[{"i", ",", "j"}], "]"}], ",", 
     RowBox[{"{", 
      RowBox[{"i", ",", "5"}], "}"}], ",", 
     RowBox[{"{", 
      RowBox[{"j", ",", "5"}], "}"}]}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.516708550491274*^9, 3.516708565841152*^9}, {
  3.5219868472809534`*^9, 3.5219868535253105`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"A", "/.", "atest"}]], "Input",
 CellChangeTimes->{{3.5167085746056533`*^9, 3.5167085760477357`*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{"1.19689396035823`", ",", "0", ",", "0", ",", "0", ",", "0"}], 
    "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
    "0.20214668746590925`", ",", "7.840374588741148`", ",", 
     "0.22483245436904015`", ",", 
     RowBox[{"-", "0.8069757285872418`"}], ",", "0.18712786224886793`"}], 
    "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", "0.0828259043763153`"}], ",", "13.315981803707265`", ",", 
     "0.20729365656915424`", ",", "4.154819403051306`", ",", 
     "0.2573791697685864`"}], "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", "0.3808535485970228`"}], ",", 
     RowBox[{"-", "10.979481912107921`"}], ",", 
     RowBox[{"-", "0.19665480379455527`"}], ",", 
     RowBox[{"-", "0.42141500655251124`"}], ",", "0.6584527300961083`"}], 
    "}"}], ",", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"-", "3.261238467195124`"}], ",", 
     RowBox[{"-", "2.0631556896285772`*^-8"}], ",", "5.819659430757391`", ",",
      "0.37746704753242283`", ",", "3.023429578158533`"}], "}"}]}], 
  "}"}]], "Output",
 CellChangeTimes->CompressedData["
1:eJwVy19MkmEYhnFoyGw5IuZwzWy2ZuSkgUGtpIiXcCsQJ+YQWTUzW7ZWRLMQ
osnCVmrMrSip4Vc4U5bWDGhtRW5GHvRnRuaYtbY8wMIvcrTBhtRmz3Nw77f7
4NrUYqo/wWQwGIdg6nj+wh0JTbLM3gv94LvqoyIPaBLXx9Bxa0IxCA4+930Z
BR9JB6LndtDkw4BqnRmU6jbKUGXp15J28I8xt2IFn6Ye77SBAuFPFZqciPiI
jCYMV0MYdSStZW6Q09b1wriHJqIlIxUEh+Ufr4VAf+tNskVOk/vB3YUCkO2s
8mwFw8taTh3ozp8JHAaVxh9z6LT+8hEHKDx5Y2YEv9ewzQ86wi1OdGLSYsjf
R5Oubge3HOxpPhhHm45VRofA0qopwTA4e7ssM6ygyebTte+zYJ78JbUM5moq
tlsITTp+FU2jDWPmRSu4Mm4+ZQOpkZK9l0CnqliDRm219+xgZ+8aP/rGm3qG
8q8GNFkwlC6wo28lWeky9p/UTqWSJufNSQ96sY/XVQ3yYgsPUXWy5zVq6K48
qwFj+lE32lhTodWCeUtcAzrv4PehxbuOF9WCn9squI2gLbGoQweE6m+ofOjf
pAEMMcVzaEfmu74JXF2wytIOXm+Ob7CCuSsZEwVKWOnOW/tpkkglRW6w0Meu
Q9ezZ8+glDviQjmJiBdN21VjKC17NYWyZA9+o16xa6gfDCgV5UoVTe7yedQB
cO1fna0VfDIfZOXA/+zYQpI=
  "]]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"yhsolve", " ", "=", " ", 
   RowBox[{
    RowBox[{"Join", "[", 
     RowBox[{
      RowBox[{"Flatten", "[", "yplus", "]"}], ",", 
      RowBox[{"{", "h", "}"}]}], "]"}], "/.", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
       RowBox[{
        SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "\[Rule]", " ", 
      RowBox[{"\[Beta]", "[", "t", "]"}]}], "}"}]}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{"MatrixForm", "[", "yhsolve", "]"}]}], "Input",
 CellChangeTimes->{{3.5159453782986727`*^9, 3.515945470066921*^9}, {
   3.515946921330929*^9, 3.515946925987195*^9}, 3.5159579961003695`*^9}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", 
   TagBox[GridBox[{
      {
       RowBox[{
        RowBox[{"-", 
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "1"}], "]"}]}], "-", 
        RowBox[{"a", "[", 
         RowBox[{"2", ",", "5"}], "]"}], "-", 
        FractionBox[
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
            "]"}]}]}]]}]},
      {
       RowBox[{
        RowBox[{"-", 
         RowBox[{"a", "[", 
          RowBox[{"3", ",", "1"}], "]"}]}], "-", 
        RowBox[{"a", "[", 
         RowBox[{"3", ",", "5"}], "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}]},
      {
       RowBox[{
        RowBox[{"-", 
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "1"}], "]"}]}], "-", 
        RowBox[{"a", "[", 
         RowBox[{"4", ",", "5"}], "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]},
      {
       RowBox[{
        RowBox[{"-", 
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "1"}], "]"}]}], "-", 
        RowBox[{"a", "[", 
         RowBox[{"5", ",", "5"}], "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}]},
      {
       FractionBox[
        RowBox[{"127", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"685", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
           RowBox[{"514", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "-", 
           RowBox[{"514", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              RowBox[{"\[Beta]", "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "-", 
           RowBox[{"685", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              RowBox[{"\[Beta]", "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
          ")"}]}], "250000"]}
     },
     GridBoxAlignment->{
      "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}},
        "RowsIndexed" -> {}},
     GridBoxSpacings->{"Columns" -> {
         Offset[0.27999999999999997`], {
          Offset[0.5599999999999999]}, 
         Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
         Offset[0.2], {
          Offset[0.4]}, 
         Offset[0.2]}, "RowsIndexed" -> {}}],
    Column], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->CompressedData["
1:eJwV0FtI02Ech/GVol4ssSHKShPt4Exj4WGQWvmakvlXZyPnaaGJRgVqibmm
koqhhGWRQ4fo8NwiQdAp4SlaJpFhIiYKE0vE8o11IM9k9f1dPHzuH++sfFXO
HpFIlI6yPSbWfQI5W/Z8nSGDIoO0xh8Gi7VVcijWtttC4N81r70K2Lb40bs+
iDPhRe3tBpjscEVugAEtA02k1VLeQhqsabPkkLuItcFnswPzz+Hmk/apmyGc
hZUk7b8FFUWyMNIWuuBZCNX8zz8dFM/0KIqhvZJHkfo8eRsL48y0mzRMbl0q
OaqH6uaqwbRwzobnNcY+2PtyqtoMH5jr2LEznI0uhbr6wsn4cIMMHq9ROidC
i+NMrwZ2zX+ZI7Oi7l4uh0dCa6efwpie9BMmqArKqSQPl+pSnM5y5pNQ4eIH
RyXCMtkvDZ7qgFcbx3274Cc33/WuCM54pnJiC2bGDRu3YftX/0Atw5cf0kmy
+n7Bqg5m1RdcL4ajs4dOl8AFlYdAqgaVjaXwg0VsIqOtv/pJhwqzsAUvLO0r
JZO8doK3YepBoTIykjMXhc1A3jnlei8aFg2tdJLej2ss5IGywDwBNsV160lj
d0B8POyolqSQNyxuj0injGz3BChd83dJhu+8vl0kc7djreTCtd1XKVDTfHKO
jG34rE6F9Zt22kKo1ax46KAwuJFvhD991svqznEm2bTJ9dDurWMiuTo9k0uO
N449JJ1HxprIuffR3aRlceQNudjXaiNl52s7GuD3jQi/yCjOJn5LjDHQFK4u
zoZxnWb7HfgfTWhjQA==
  "]]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"MatrixForm", "[", 
  RowBox[{"yhsolve", "\[LeftDoubleBracket]", 
   RowBox[{"{", 
    RowBox[{"2", ",", "3", ",", "1", ",", "5", ",", "4"}], "}"}], 
   "\[RightDoubleBracket]"}], "]"}]], "Input",
 CellChangeTimes->{{3.515892100226381*^9, 3.515892103605574*^9}, {
  3.515945478404398*^9, 3.5159455056989594`*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", 
   TagBox[GridBox[{
      {
       RowBox[{
        RowBox[{"-", 
         RowBox[{"a", "[", 
          RowBox[{"3", ",", "1"}], "]"}]}], "-", 
        RowBox[{"a", "[", 
         RowBox[{"3", ",", "5"}], "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}]},
      {
       RowBox[{
        RowBox[{"-", 
         RowBox[{"a", "[", 
          RowBox[{"4", ",", "1"}], "]"}]}], "-", 
        RowBox[{"a", "[", 
         RowBox[{"4", ",", "5"}], "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]},
      {
       RowBox[{
        RowBox[{"-", 
         RowBox[{"a", "[", 
          RowBox[{"2", ",", "1"}], "]"}]}], "-", 
        RowBox[{"a", "[", 
         RowBox[{"2", ",", "5"}], "]"}], "-", 
        FractionBox[
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}]}], 
         RowBox[{
          RowBox[{"685", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
          RowBox[{"514", " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
             RowBox[{
              SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], 
            "]"}]}]}]]}]},
      {
       FractionBox[
        RowBox[{"127", " ", 
         RowBox[{"(", 
          RowBox[{
           RowBox[{"685", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
           RowBox[{"514", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "-", 
           RowBox[{"514", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              RowBox[{"\[Beta]", "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "-", 
           RowBox[{"685", " ", 
            RowBox[{"Cos", "[", 
             RowBox[{
              RowBox[{"\[Beta]", "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "-", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
          ")"}]}], "250000"]},
      {
       RowBox[{
        RowBox[{"-", 
         RowBox[{"a", "[", 
          RowBox[{"5", ",", "1"}], "]"}]}], "-", 
        RowBox[{"a", "[", 
         RowBox[{"5", ",", "5"}], "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}]}
     },
     GridBoxAlignment->{
      "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}},
        "RowsIndexed" -> {}},
     GridBoxSpacings->{"Columns" -> {
         Offset[0.27999999999999997`], {
          Offset[0.5599999999999999]}, 
         Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
         Offset[0.2], {
          Offset[0.4]}, 
         Offset[0.2]}, "RowsIndexed" -> {}}],
    Column], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{{3.5158920906568336`*^9, 3.515892103941593*^9}, 
   3.515894251870448*^9, 3.5158942865134296`*^9, 3.515894327309763*^9, 
   3.515894727905676*^9, 3.515895140546277*^9, {3.515895196225462*^9, 
   3.515895215001536*^9}, {3.5158958853858795`*^9, 3.515895898747644*^9}, 
   3.515896800193204*^9, 3.515897625598414*^9, 3.5159455061889873`*^9, 
   3.5159461116406174`*^9, 3.5159580155244803`*^9, 3.5159645082180004`*^9, 
   3.5159726597702427`*^9, 3.5159857731522846`*^9, 3.5159859387297554`*^9, 
   3.516707086759553*^9, 3.5167098045720034`*^9, 3.516711291119029*^9, 
   3.516712626927433*^9, 3.516731912075901*^9, 3.516751565777707*^9, 
   3.5176792371846943`*^9, 3.517679754760298*^9, 3.5176800893654366`*^9, 
   3.5176859990124483`*^9, 3.5176980479846106`*^9, 3.517698485554638*^9, 
   3.5176985994171505`*^9, 3.520728341318468*^9, 3.5207285467292166`*^9, 
   3.5208973238991246`*^9, 3.5213040202557087`*^9, 3.521486066962582*^9, 
   3.521487934570403*^9, 3.522245132659792*^9, 3.5222468178771806`*^9, 
   3.5222499219405003`*^9, 3.5223307385347404`*^9, 3.522356630688347*^9, 
   3.5223569353077183`*^9, 3.522429675974849*^9, 3.522510484503601*^9, 
   3.5225112109541516`*^9, 3.522511899235519*^9, 3.5227081638504066`*^9, 
   3.5227751720677896`*^9, 3.522776866401041*^9, 3.5230286097211437`*^9, 
   3.5230313582493505`*^9, 3.523557289268545*^9, 3.5237248905105815`*^9, 
   3.5237265819183245`*^9, 3.5239837613931494`*^9, 3.5239850088585*^9, 
   3.5239893236052895`*^9, 3.523990347607859*^9, 3.5239920409987154`*^9, 
   3.523992152918117*^9, 3.5239948345304966`*^9, 3.523994944437783*^9, 
   3.5239950555951405`*^9, 3.5242481576947527`*^9, 3.5242485444628744`*^9, 
   3.5242500238793287`*^9, 3.524400154356089*^9, 3.524400327461852*^9, 
   3.5244063046750183`*^9, 3.524406575240385*^9, 3.5244068044013443`*^9, 
   3.5244184176580157`*^9, 3.5244187254123535`*^9, 3.5244222494390736`*^9, 
   3.5244223366260605`*^9, 3.5244227767172327`*^9, 3.5244238671726027`*^9, 
   3.524442241392548*^9, 3.5244427673856335`*^9, 3.5244438743699493`*^9, 
   3.524445711944052*^9, 3.52444588953821*^9, 3.5244469031027207`*^9, 
   3.524502456182976*^9, 3.5245119752527905`*^9, 3.5245884157482433`*^9, 
   3.5250903036785154`*^9, 3.525091711469412*^9, 3.5250919682811675`*^9, 
   3.525092206673747*^9, 3.5250924621748953`*^9, 3.52509258217408*^9, 
   3.5250927062222576`*^9, 3.525092909963564*^9, 3.52509322909921*^9, 
   3.5250987884861884`*^9, 3.525448010053144*^9, 3.525461232625262*^9, 
   3.5255182907289066`*^9, 3.52582454967918*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"Theta5Cond", "=", 
   RowBox[{
    RowBox[{"Solve", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
       "yhsolve", "\[LeftDoubleBracket]", "2", "\[RightDoubleBracket]"}], 
       "\[Equal]", "0"}], ",", 
      RowBox[{
       SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}], 
    "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Theta2Cond", "=", 
   RowBox[{
    RowBox[{"Solve", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
       "yhsolve", "\[LeftDoubleBracket]", "3", "\[RightDoubleBracket]"}], 
       "\[Equal]", "0"}], ",", 
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], 
    "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Theta1Cond", "=", 
   RowBox[{
    RowBox[{"Solve", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
       "yhsolve", "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}], 
       "\[Equal]", "0"}], ",", 
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "]"}], 
    "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"ThetabetaCond", "=", 
   RowBox[{
    RowBox[{"Solve", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
       "yhsolve", "\[LeftDoubleBracket]", "5", "\[RightDoubleBracket]"}], 
       "\[Equal]", "0"}], ",", 
      RowBox[{"\[Beta]", "[", "t", "]"}]}], "]"}], "\[LeftDoubleBracket]", 
    "2", "\[RightDoubleBracket]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Theta3Cond", "=", 
   RowBox[{
    RowBox[{"Solve", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
       "yhsolve", "\[LeftDoubleBracket]", "4", "\[RightDoubleBracket]"}], 
       "\[Equal]", "0"}], ",", 
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "]"}], 
    "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{
    RowBox[{
     RowBox[{
      RowBox[{
       RowBox[{
        RowBox[{
         RowBox[{
          RowBox[{
           RowBox[{"Transpose", "[", 
            RowBox[{"{", 
             RowBox[{"{", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], ",", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], ",", 
               RowBox[{
                SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], ",", 
               RowBox[{
                RowBox[{
                 SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                RowBox[{"\[Beta]", "[", "t", "]"}]}], ",", 
               RowBox[{
                SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], 
             "}"}], "]"}], "/.", "\[InvisibleSpace]", "Theta3Cond"}], "/.", 
          "\[InvisibleSpace]", "atest"}], "/.", "\[InvisibleSpace]", 
         "ThetabetaCond"}], "/.", "\[InvisibleSpace]", "atest"}], "/.", 
       "\[InvisibleSpace]", "Theta1Cond"}], "/.", "\[InvisibleSpace]", 
      "atest"}], "/.", "\[InvisibleSpace]", "Theta5Cond"}], "/.", 
    "\[InvisibleSpace]", "atest"}], "/.", "\[InvisibleSpace]", "Theta2Cond"}],
   "/.", "\[InvisibleSpace]", "atest"}]}], "Input",
 CellChangeTimes->{{3.51594676032572*^9, 3.5159468099305573`*^9}, {
   3.5159566741957607`*^9, 3.51595678713122*^9}, {3.5159568457875752`*^9, 
   3.5159568538670373`*^9}, {3.515956895125397*^9, 3.5159569150995398`*^9}, {
   3.5159571238684807`*^9, 3.515957170348139*^9}, {3.515958003729806*^9, 
   3.515958007675031*^9}, {3.5159638958349743`*^9, 3.515963896592017*^9}, {
   3.51596399752379*^9, 3.51596400259408*^9}, {3.515964082366643*^9, 
   3.515964102957821*^9}, {3.5159641483604174`*^9, 3.515964231857193*^9}, {
   3.515964279459916*^9, 3.5159642822650766`*^9}, {3.5159728721103873`*^9, 
   3.5159729359940414`*^9}, {3.515973121749666*^9, 3.5159731262559233`*^9}, 
   3.516711531312767*^9, {3.52072837720152*^9, 3.520728378225579*^9}, {
   3.522511106163158*^9, 3.5225111066501856`*^9}, {3.522511147690533*^9, 
   3.5225111497466507`*^9}, {3.5225118207090273`*^9, 3.522511820941041*^9}}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{
   RowBox[{"{", 
    RowBox[{"-", "2.6514877315488175`"}], "}"}], ",", 
   RowBox[{"{", "0.27759918149908547`", "}"}], ",", 
   RowBox[{"{", "2.136079661013141`", "}"}], ",", 
   RowBox[{"{", "0.8510021438341864`", "}"}], ",", 
   RowBox[{"{", "0.17455326539227106`", "}"}]}], "}"}]], "Output",
 CellChangeTimes->{{3.515964159175036*^9, 3.5159642329842577`*^9}, 
   3.5159642830411205`*^9, 3.5159726617303543`*^9, 3.5159727592559323`*^9, {
   3.5159728696132445`*^9, 3.5159729367200828`*^9}, {3.5159731227897253`*^9, 
   3.5159731277000065`*^9}, 3.5159734080060387`*^9, 3.5159737266572647`*^9, 
   3.5159857780885673`*^9, 3.515985944493085*^9, 3.5167070882696395`*^9, 
   3.516709806406108*^9, 3.5167112928461275`*^9, 3.5167122145968485`*^9, 
   3.5167126286265297`*^9, 3.5167319136439905`*^9, 3.5167515672867937`*^9, 
   3.5176792411009183`*^9, 3.5176797563663893`*^9, 3.517680091134537*^9, 
   3.5176860006535425`*^9, 3.517698049568701*^9, 3.517698487197732*^9, 
   3.5176986011342487`*^9, {3.520728361759637*^9, 3.520728379207635*^9}, 
   3.5207285477412744`*^9, 3.520897325058191*^9, 3.521304021449777*^9, 
   3.521486068197653*^9, 3.52148793555746*^9, 3.522245133550843*^9, 
   3.5222468187482305`*^9, 3.5222499231065674`*^9, 3.5223307403468437`*^9, 
   3.522356631655549*^9, 3.52235693622812*^9, 3.5224296770979137`*^9, 
   3.5225104858696795`*^9, 3.5225111085472946`*^9, {3.522511150420689*^9, 
   3.522511160592271*^9}, 3.5225112124152355`*^9, 3.5225119004915905`*^9, 
   3.522708165066476*^9, 3.522775172848834*^9, 3.5227768677071157`*^9, 
   3.523028610939213*^9, 3.523031359257408*^9, 3.5235572904366117`*^9, 
   3.5237248913266287`*^9, 3.5237265827213707`*^9, 3.5239837621691933`*^9, 
   3.523985009654546*^9, 3.523989324383334*^9, 3.5239903483939037`*^9, 
   3.5239920417877607`*^9, 3.5239921537041616`*^9, 3.523994835339543*^9, 
   3.5239949452308283`*^9, 3.523995056382186*^9, 3.5242481585117993`*^9, 
   3.5242485452519197`*^9, 3.5242500253394117`*^9, 3.5244001551201324`*^9, 
   3.5244003282268963`*^9, 3.5244063054780636`*^9, 3.5244065760114255`*^9, 
   3.5244068051893673`*^9, 3.524418418448017*^9, 3.524418726211396*^9, 
   3.52442225025712*^9, 3.5244223374451075`*^9, 3.524422777508278*^9, 
   3.524423867975649*^9, 3.5244422421665926`*^9, 3.5244427681956797`*^9, 
   3.5244438751769953`*^9, 3.5244457127200966`*^9, 3.5244458903142543`*^9, 
   3.524446904087777*^9, 3.524502457392045*^9, 3.5245119767248745`*^9, 
   3.5245884165652895`*^9, 3.52509030445756*^9, 3.5250917122594547`*^9, 
   3.5250919690652122`*^9, 3.5250922074457912`*^9, 3.5250924629539375`*^9, 
   3.525092582946124*^9, 3.5250927070153027`*^9, 3.5250929107445884`*^9, 
   3.52509323015527*^9, 3.525098789498246*^9, 3.5254480110241995`*^9, 
   3.525461233431308*^9, 3.5255182925120087`*^9, 3.525824550994255*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{"Theta1a", " ", "=", " ", 
  RowBox[{
   RowBox[{
    SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "/.", 
   "Theta1Cond"}]}], "\[IndentingNewLine]", 
 RowBox[{"Theta2a", " ", "=", " ", 
  RowBox[{
   RowBox[{
    SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "/.", 
   "Theta2Cond"}]}], "\[IndentingNewLine]", 
 RowBox[{"Theta3a", " ", "=", " ", 
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "/.", "Theta3Cond"}], "/.",
    "ThetabetaCond"}]}], "\[IndentingNewLine]", 
 RowBox[{"Theta4a", " ", "=", " ", 
  RowBox[{
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], " ", "-", 
     RowBox[{"\[Beta]", "[", "t", "]"}]}], "/.", "Theta3Cond"}], "/.", 
   "ThetabetaCond"}]}], "\[IndentingNewLine]", 
 RowBox[{"Theta5a", " ", "=", " ", 
  RowBox[{
   RowBox[{
    SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "/.", 
   "Theta5Cond"}]}]}], "Input",
 CellChangeTimes->{{3.515957319602676*^9, 3.515957408978788*^9}, {
   3.5159580291182575`*^9, 3.51595803894882*^9}, {3.515958695734386*^9, 
   3.5159587502905064`*^9}, 3.51595934042026*^9, {3.515963224972603*^9, 
   3.5159632256866436`*^9}, {3.515963471284691*^9, 3.515963496007105*^9}, {
   3.516712231378809*^9, 3.516712233642938*^9}}],

Cell[BoxData[
 RowBox[{"-", 
  RowBox[{"ArcCos", "[", 
   RowBox[{"-", 
    RowBox[{
     RowBox[{"(", 
      RowBox[{"685", "+", 
       RowBox[{"514", " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}]}], "-", 
       RowBox[{"514", " ", 
        RowBox[{"a", "[", 
         RowBox[{"2", ",", "1"}], "]"}], " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}]}], "-", 
       RowBox[{"514", " ", 
        RowBox[{"a", "[", 
         RowBox[{"2", ",", "5"}], "]"}], " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}]}]}], ")"}], 
     "/", 
     RowBox[{"(", 
      RowBox[{"\[Sqrt]", 
       RowBox[{"(", 
        RowBox[{"469225", "+", 
         RowBox[{"469225", " ", 
          SuperscriptBox[
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "1"}], "]"}], "2"]}], "+", 
         RowBox[{"938450", " ", 
          RowBox[{"a", "[", 
           RowBox[{"2", ",", "1"}], "]"}], " ", 
          RowBox[{"a", "[", 
           RowBox[{"2", ",", "5"}], "]"}]}], "+", 
         RowBox[{"469225", " ", 
          SuperscriptBox[
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "5"}], "]"}], "2"]}], "+", 
         RowBox[{"704180", " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}]}], "+", 
         RowBox[{"704180", " ", 
          SuperscriptBox[
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "1"}], "]"}], "2"], " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}]}], "+", 
         RowBox[{"1408360", " ", 
          RowBox[{"a", "[", 
           RowBox[{"2", ",", "1"}], "]"}], " ", 
          RowBox[{"a", "[", 
           RowBox[{"2", ",", "5"}], "]"}], " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}]}], "+", 
         RowBox[{"704180", " ", 
          SuperscriptBox[
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "5"}], "]"}], "2"], " ", 
          RowBox[{"Cos", "[", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}]}], "+", 
         RowBox[{"264196", " ", 
          SuperscriptBox[
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}], "2"]}], 
         "+", 
         RowBox[{"264196", " ", 
          SuperscriptBox[
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "1"}], "]"}], "2"], " ", 
          SuperscriptBox[
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}], "2"]}], 
         "+", 
         RowBox[{"528392", " ", 
          RowBox[{"a", "[", 
           RowBox[{"2", ",", "1"}], "]"}], " ", 
          RowBox[{"a", "[", 
           RowBox[{"2", ",", "5"}], "]"}], " ", 
          SuperscriptBox[
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}], "2"]}], 
         "+", 
         RowBox[{"264196", " ", 
          SuperscriptBox[
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "5"}], "]"}], "2"], " ", 
          SuperscriptBox[
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}], "2"]}], 
         "+", 
         RowBox[{"264196", " ", 
          SuperscriptBox[
           RowBox[{"Sin", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}], "2"]}], 
         "+", 
         RowBox[{"264196", " ", 
          SuperscriptBox[
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "1"}], "]"}], "2"], " ", 
          SuperscriptBox[
           RowBox[{"Sin", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}], "2"]}], 
         "+", 
         RowBox[{"528392", " ", 
          RowBox[{"a", "[", 
           RowBox[{"2", ",", "1"}], "]"}], " ", 
          RowBox[{"a", "[", 
           RowBox[{"2", ",", "5"}], "]"}], " ", 
          SuperscriptBox[
           RowBox[{"Sin", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}], "2"]}], 
         "+", 
         RowBox[{"264196", " ", 
          SuperscriptBox[
           RowBox[{"a", "[", 
            RowBox[{"2", ",", "5"}], "]"}], "2"], " ", 
          SuperscriptBox[
           RowBox[{"Sin", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}], "2"]}]}], 
        ")"}]}], ")"}]}]}], "]"}]}]], "Output",
 CellChangeTimes->CompressedData["
1:eJwVzl1I02EYxuG5ipZpLhM3Bg2k/BhYLk1Lt7m9S0dszNaC3MKKxbQstyYM
bGviwCIiRoULV40Rtv1nZSAzPJAhOZgSmmhozQql0GBvhX2YbQXR8xzcXNxn
v4IzF/XNGSwW6yhMmKwN+Soomcx89BrdfVxJ+kC7LrXwBFS3yWdtlZQsj93f
3g4uVnRKUEmgQGgHsz/IWE7wxsrfKnSeVNejvz9PPCUSSvwrjijaW/qw0Av6
PJGRE1JKBqXXA0Pgv61r156BLfEhUlRLSdpozCsGR5uafSWgTeXZpkPT65Em
sHCKs4AmFMGTbpCd//xVGMy6emtPP2iduN2NXhKEDBw5JZk5fq4IzBc7ltGD
ueaZICgtocUMmPVW94tRUCIotUymwBZpIpAGVT9V5R2Ekmh11TT6WORJOkCF
29PqBJm2GtllUBo+oEG/xC7cc4Falagf7ZzLHkb5P2Y1KTBPKHKhn3h7K9Og
etHcrVRCp4lzF/UOiK/Ug6ePZTBo7qZwDB1bPWLVgOPKF160z9eg1YLtljID
OsPfdxP9vtbFawDj8ypuI6h3btaj9tGz79HEG17MAJ4fUSdQ1vqGRiP4LVne
YQddrfydDvzWHbYAyLzLcfccoqROsEXsBYtku3RolLtqQdl3ljxozf4lP6p4
aRpAh7sScfRUZPwrOjc9GOwFz02ZRMo6SuQPygKHQTbT4zSDodjHjX/A/5Y0
NGc=
  "]],

Cell[BoxData[
 RowBox[{
  RowBox[{"a", "[", 
   RowBox[{"4", ",", "1"}], "]"}], "+", 
  RowBox[{"a", "[", 
   RowBox[{"4", ",", "5"}], "]"}]}]], "Output",
 CellChangeTimes->CompressedData["
1:eJwVzl1I02EYxuG5ipZpLhM3Bg2k/BhYLk1Lt7m9S0dszNaC3MKKxbQstyYM
bGviwCIiRoULV40RNv+zMpAZHsiQHEwJTTS0ZoVSaLC3wj7MtgTpeQ5uLu6z
X965y/rGNBaLdRwmTFT3+soomUh/8hbde1JJekC7Ljn/DFS3yGds5ZQsjT7c
2QoulLVLUEkgT2gHMz/JWE7w1vJ6BTpHKmvRv1/HnxMJJf5lRwTtLn6c7wV9
nvDwKSklA9KbgUFwY/vqjRdgU2yQFFRTkjIacwrBkYZGXxFoU3l26NDUWrgB
zJ/kzKNxRfC0G2TnvnwTAjOu39nXB1rH73aiVwS9Bo6ckvQsP1cE5oodS+jh
bPN0EJQW0UIGzHiv+8MoKBEUWyaSYJM0HkiBqt+q0jZCSaSyYgp9KvIkHKDC
7Wl2gkxLlewqKA0d0qDfopceuECtStSHts9mDqH8XzOaJJgjFLnQL7z95SlQ
vWDuVCqh08S5j3r7xddqwbMn0hg0e0soio6uHLNqwDHlKy/a46vTasFWS4kB
neYfuI3+XO3g1YGxORW3HtQ7t+pR+8j5j2j8HS9qAC8Oq+Moa21TvRH8kSht
s4OuZv5uB37rLlsAZD5kubuOUFIj2Cb2ggWyPTo0wl2xoOx7ix606uCiH1W8
NvWjQx3xGHomPPYdnZ0aCHaDFyZNImUNJfJHJYGjIJvpcprBjfXPm/+B/wGa
izT5
  "]],

Cell[BoxData[
 RowBox[{
  RowBox[{"a", "[", 
   RowBox[{"5", ",", "1"}], "]"}], "+", 
  RowBox[{"a", "[", 
   RowBox[{"5", ",", "5"}], "]"}], "-", 
  RowBox[{
   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "-", 
  RowBox[{
   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]], "Output",
 CellChangeTimes->CompressedData["
1:eJwVzl1I02EYxuG5ipbNXCZuDBpI+TGwXJqWbnN7l45wzNaC3MI+FtOy3Jow
sK2JA4uIGBUuXDWG2PzPykBmeCBDcjAlNNHQmhVKpMHeCvsw2wqi5zm4ubjP
frlnL+kb01gs1lGYKFHV5yulZDL90Wt093EV6QXtuuTCE7C2RTFrK6NkeezB
9lZwsbRdikoDuSI7mPFBznKCN1f+lqPzpKIG/f154imRUuJfcUTQ7qKHeV7Q
5wmPnJBRMii7ERgC/21du/4MbIoNkfwqSlJGY3YBONrQ6CsEbWrPNh2aWg83
gHlTnAU0rgyedIPsnOevQiD32u09/aB14k4nelnYZ+AoKEnP9PPEYI7EsYwe
zDLPBEFZIS1gQO5b3S9GSYmwyDKZBJtk8UAKVP9Ul7QRSiIV5dPoY7En4QCV
bk+zE2RaKuVXQFnogAb9Er143wVq1eJ+tH0uYxgV/JjVJMFskdiFfuLvLUuB
tYvmTpUKOk2ce6h3QHK1Bjx9LI1BszaFoujY6hGrBhxXvfCivb46rRZstRQb
0BnBvlvo97UOfh0Ym1fz6kG9c7MetY+ee4/G3/CjBvDCSG0cZa1vqDeC3xIl
bXbQ1SzY6cBv3WELgMy7THfXIUqqhVskXjBfvkuHRnirFpR9d8mDVu5f8qPK
l6YBdLgjHkNPhce/onPTg8Fu8PyUSayqpkTRUxw4DLKZLqcZPMP9uPEP+B+P
GDNt
  "]],

Cell[BoxData[
 RowBox[{
  RowBox[{"a", "[", 
   RowBox[{"5", ",", "1"}], "]"}], "+", 
  RowBox[{"a", "[", 
   RowBox[{"5", ",", "5"}], "]"}], "-", 
  RowBox[{"ArcCos", "[", 
   RowBox[{
    RowBox[{"(", 
     RowBox[{"528392", "+", 
      RowBox[{"704180", " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}]}], "+", 
      RowBox[{"704180", " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         RowBox[{"2", " ", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "+", 
      RowBox[{"528392", " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         RowBox[{"2", " ", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
         RowBox[{"2", " ", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]}], "]"}]}], "+", 
      RowBox[{"938450", " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "+", 
      RowBox[{"938450", " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         RowBox[{"2", " ", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "+", 
      RowBox[{"704180", " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         RowBox[{"2", " ", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
         RowBox[{"2", " ", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], "+", 
      RowBox[{"704180", " ", 
       RowBox[{"Cos", "[", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}], "-", 
      RowBox[{"\[Sqrt]", 
       RowBox[{"(", 
        RowBox[{
         SuperscriptBox[
          RowBox[{"(", 
           RowBox[{
            RowBox[{"-", "528392"}], "-", 
            RowBox[{"704180", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}]}], "-", 
            RowBox[{"704180", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               RowBox[{"2", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], 
            "-", 
            RowBox[{"528392", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               RowBox[{"2", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
               RowBox[{"2", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]}], "]"}]}], 
            "-", 
            RowBox[{"938450", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], 
            "-", 
            RowBox[{"938450", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               RowBox[{"2", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], 
            "-", 
            RowBox[{"704180", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               RowBox[{"2", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
               RowBox[{"2", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], 
            "-", 
            RowBox[{"704180", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], 
           ")"}], "2"], "-", 
         RowBox[{"4", " ", 
          RowBox[{"(", 
           RowBox[{
            RowBox[{"469225", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{"2", " ", 
               RowBox[{
                SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "]"}]}], 
            "+", 
            RowBox[{"704180", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "]"}]}], "+", 
            RowBox[{"704180", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               RowBox[{"2", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
               RowBox[{
                SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], 
            "+", 
            RowBox[{"528392", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               RowBox[{"2", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
               RowBox[{"2", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]}], "]"}]}], 
            "+", 
            RowBox[{"469225", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               RowBox[{"2", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
               RowBox[{"2", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "-", 
               RowBox[{"2", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}]}], "]"}]}], 
            "+", 
            RowBox[{"704180", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               RowBox[{"2", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "+", 
               RowBox[{"2", " ", 
                RowBox[{
                 SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "-", 
               RowBox[{
                SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], 
            "-", 
            RowBox[{"704180", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], 
           ")"}], " ", 
          RowBox[{"(", 
           RowBox[{"1466842", "+", 
            RowBox[{"1408360", " ", 
             RowBox[{"Cos", "[", 
              RowBox[{
               SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], 
           ")"}]}]}], ")"}]}]}], ")"}], "/", 
    RowBox[{"(", 
     RowBox[{"2", " ", 
      RowBox[{"(", 
       RowBox[{"1466842", "+", 
        RowBox[{"1408360", " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "]"}]}]}], ")"}]}],
      ")"}]}], "]"}], "-", 
  RowBox[{
   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "-", 
  RowBox[{
   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}]], "Output",
 CellChangeTimes->CompressedData["
1:eJwVzl1I02EYxuG5ipZpLhM3Rg2k/BhYLk1Lt7m9S0dszNYC3cKKxbQstyYM
bGviwCIiRoULV40RNv+zMpAZHsiQHEwJTTS0ZoUSabC3wj7MtoLoeQ5uLu6z
X96Zi/qmNBaLdQwmTFT3+coomUx/9BrdU68kvaBdl1x4Aqpb5bO2ckqWx+5v
bwMXyzokqCSQJ7SDmR9kLCd4Y+VvBTpPKmvR358nnhIJJf4VRwTtKX6Y7wV9
nvDICSklg9LrgSHw39a1a8/A5tgQKaimJGU05hSCo41NviLQpvJs06Gp9XAj
mD/FWUDjiuBJN8jOff4qBGZcvbW3H7RO3O5CLwn6DBw5JelZfq4IzBU7ltFD
2eaZICgtooUMmPFW94tRUCIotkwmwWZpPJACVT9Vpe2EkkhlxTT6WORJOECF
29PiBJnWKtllUBo6qEG/RC/cc4Falagf7ZjLHEb5P2Y1STBHKHKhn3j7ylOg
etHcpVRCp4lzF/UOiK/UgqePpzFo9qZQFB1bPWrVgOPKF16011en1YJtlhID
OsPffxP9vtbJqwNj8ypuA6h3btaj9tGz79H4G17UAJ4fUcdR1vqGBiP4LVHa
bgddLfxdDvzWHbYAyLzLcncfpqRGsEXsBQtku3VohLtqQdl3ljxo1YElP6p4
aRpAhzvjMfRUePwrOjc9GOwBz02ZRMoaSuQPSgJHQDbT7TSDO+s/bvwD/geO
/jNz
  "]],

Cell[BoxData[
 RowBox[{
  RowBox[{"a", "[", 
   RowBox[{"3", ",", "1"}], "]"}], "+", 
  RowBox[{"a", "[", 
   RowBox[{"3", ",", "5"}], "]"}]}]], "Output",
 CellChangeTimes->CompressedData["
1:eJwVzl1I02EYxuG5ipZpLhM3Bg2k/BhYLk1Lt7m9a45wzNaC3MKKxbQstyYM
bGviwEIiRoULV40ROv+zMpAZHsiQHEwJTTS0ZoUSabC3wj7MtoLoeQ5uLu6z
X965y/rGNBaLdRwmTFT3+8oomUp/9Brde1JJekG7Lrn4BKxtkc/ZyilZGX+w
sxVcKmuXoJJAntAOZn6QsZzgzdW/FegCqaxBf3+efEoklPhXHRG0p7gv3wv6
POHRU1JKhqQ3AsPgv+3rXc/AptgwKaimJGU05hSCYw2NviLQpvbs0KGpjXAD
mD/NWUTjiuBpN8jOff4qBGZcv71vALRO3ulErwj6DRw5JelZfq4IzBU7VtDD
2ebZICgtooUMmPFW94tRUCIotkwlwSZpPJAC1T/VpW2EkkhlxQz6WORJOECF
29PsBJmWKtlVUBo6pEG/RC/dd4FatWgAbZ/PHEH5P+Y0STBHKHKhn3j7y1Ng
7ZK5U6mEThPnHuodFF+rAc+eSGPQ7C2hKDq+dsyqASeUL7xor69OqwVbLSUG
dJZ/4Bb6fb2DVwfGFtTcelDv3KpH7WPn36PxN7yoAbw4WhtHWRub6o3gt0Rp
mx10NfN3O/Bbd9kCIPMuy919hBKVYJvYCxbI9ujQCHfNgrLvLnvQqoPLflTx
0jSIjnTEY+iZ8MRXdH5mKNgDXpg2iZQqSuQPSwJHQTbT7TSDqq6Pm/+A/wGR
lTPM
  "]]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<theta_a1\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Theta1a", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{
  3.5158922845389233`*^9, {3.5158938060489483`*^9, 3.515893808904112*^9}, {
   3.5159574775057077`*^9, 3.515957506890388*^9}, {3.5159638351495028`*^9, 
   3.515963852128474*^9}, 3.5167122415113883`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<theta_a2\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Theta2a", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{
  3.5158922845389233`*^9, {3.5158938060489483`*^9, 3.515893808904112*^9}, {
   3.5159574775057077`*^9, 3.515957506890388*^9}, {3.5159638351495028`*^9, 
   3.515963852128474*^9}, {3.5167122415113883`*^9, 3.516712243727515*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<theta_a3\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Theta3a", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{
  3.5158922845389233`*^9, {3.5158938060489483`*^9, 3.515893808904112*^9}, {
   3.5159574775057077`*^9, 3.515957506890388*^9}, {3.5159638351495028`*^9, 
   3.515963852128474*^9}, {3.5167122415113883`*^9, 3.516712245702628*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<theta_a4\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Theta4a", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{
  3.5158922845389233`*^9, {3.5158938060489483`*^9, 3.515893808904112*^9}, {
   3.5159574775057077`*^9, 3.515957506890388*^9}, {3.5159638351495028`*^9, 
   3.515963852128474*^9}, {3.5167122415113883`*^9, 3.5167122473267207`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<theta_a5\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Theta5a", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{
  3.5158922845389233`*^9, {3.5158938060489483`*^9, 3.515893808904112*^9}, {
   3.5159574775057077`*^9, 3.515957506890388*^9}, {3.5159638351495028`*^9, 
   3.515963852128474*^9}, {3.5167122415113883`*^9, 3.5167122473267207`*^9}}]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["Velocity of Outputs: Pre and Post Impact", "Subsection",
 CellChangeTimes->{{3.5133462255725746`*^9, 3.5133462845249467`*^9}, {
  3.5140475842599964`*^9, 3.514047587214165*^9}, {3.514050381757004*^9, 
  3.5140503867632904`*^9}, {3.5140508345048995`*^9, 3.514050836366006*^9}, {
  3.515340123141806*^9, 3.515340124962953*^9}}],

Cell["compute the jacobian of the output functions", "Text",
 CellChangeTimes->{{3.514056134624614*^9, 3.514056152522016*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"Y", "=", 
   RowBox[{"Join", "[", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"{", 
       SubscriptBox["p", "hip"], "}"}]}], ",", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", "2"], "]"}]}], "]"}]}], "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5158871007064247`*^9, 3.515887101970497*^9}, 
   3.5158874557787333`*^9, {3.515898875609911*^9, 3.5158988760079336`*^9}, {
   3.5158993719242983`*^9, 3.5158993862991204`*^9}, {3.515899472917075*^9, 
   3.5158994757032337`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"Yplus", "=", 
   RowBox[{
    RowBox[{
     RowBox[{"Y", "/.", "initialtime"}], "/.", "constsubs"}], "/.", 
    "resetmap"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5158905547469845`*^9, 3.5158905560910616`*^9}, 
   3.515891153092208*^9, {3.515893378403488*^9, 3.5158933836337876`*^9}, 
   3.527197738141684*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<H_plus\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{"Yplus", "/.", "statesubs"}], "/.", "constsubs"}]}], "]"}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158948955412636`*^9, 3.5158949312793083`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"Yminus", "=", 
   RowBox[{
    RowBox[{"Y", "/.", "constsubs"}], "/.", "resetmap"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5158912072393045`*^9, 3.515891227677474*^9}, {
   3.5158934248751464`*^9, 3.515893427941322*^9}, 3.5271977290383673`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<H_minus\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Yminus", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.515895085079105*^9, 3.5158950853751216`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{"h", "\[IndentingNewLine]", 
 RowBox[{"Dh", " ", "=", " ", 
  RowBox[{
   SubscriptBox["\[PartialD]", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
   RowBox[{"{", "h", "}"}]}]}]}], "Input",
 CellChangeTimes->{{3.5158876222012525`*^9, 3.5158876466726522`*^9}, {
  3.515895637521703*^9, 3.5158956394738145`*^9}}],

Cell[BoxData[
 RowBox[{
  FractionBox["1", "250000"], 
  RowBox[{"127", " ", 
   RowBox[{"(", 
    RowBox[{
     RowBox[{"685", " ", 
      RowBox[{"Cos", "[", 
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "+", 
     RowBox[{"514", " ", 
      RowBox[{"Cos", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "-", 
     RowBox[{"514", " ", 
      RowBox[{"Cos", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "-", 
     RowBox[{"685", " ", 
      RowBox[{"Cos", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
    ")"}]}]}]], "Output",
 CellChangeTimes->{{3.5158956386727686`*^9, 3.5158956398638363`*^9}, {
   3.5158958866799536`*^9, 3.515895900344735*^9}, 3.515896802422331*^9, 
   3.515897627033496*^9, 3.5159462523356647`*^9, 3.515985921767785*^9, 
   3.516707287973062*^9, 3.5167100467628555`*^9, 3.5167114899284*^9, 
   3.516712848515107*^9, 3.5167321046199136`*^9, 3.5167517629159827`*^9, 
   3.517679445919633*^9, 3.5176799540956993`*^9, 3.517680290765956*^9, 
   3.517686202845107*^9, 3.517698252865329*^9, 3.517698806692006*^9, 
   3.520728549140354*^9, 3.5208973266942844`*^9, 3.521304022935862*^9, 
   3.521486069771743*^9, 3.521487936998542*^9, 3.52224513490092*^9, 
   3.522246821400382*^9, 3.5222499247016582`*^9, 3.522330741871931*^9, 
   3.5223566332623515`*^9, 3.522356937710122*^9, 3.5224296785949993`*^9, 
   3.5225106422336226`*^9, 3.5225114183960166`*^9, 3.522511902505706*^9, 
   3.522708167077591*^9, 3.52277517434692*^9, 3.5227768699202423`*^9, 
   3.523028613473358*^9, 3.5230313612175198`*^9, 3.523557292400724*^9, 
   3.5237248928757167`*^9, 3.5237265842264566`*^9, 3.5239837637362833`*^9, 
   3.523985011164632*^9, 3.523989325828417*^9, 3.523990349878989*^9, 
   3.523992043299847*^9, 3.5239921551712456`*^9, 3.523994836828628*^9, 
   3.523994946803918*^9, 3.523995057892272*^9, 3.524248160081889*^9, 
   3.5242485466930017`*^9, 3.5242500287246056`*^9, 3.5244001564861712`*^9, 
   3.5244003296869793`*^9, 3.524406307005149*^9, 3.524406577435484*^9, 
   3.524406806634446*^9, 3.524418419968019*^9, 3.5244187277574553`*^9, 
   3.524422251980219*^9, 3.5244223391402044`*^9, 3.524422778897357*^9, 
   3.524423869471734*^9, 3.5244422436216755`*^9, 3.524442769671764*^9, 
   3.5244438766880817`*^9, 3.5244457141591787`*^9, 3.5244458917423363`*^9, 
   3.524446905993886*^9, 3.524502459654175*^9, 3.5245119778979416`*^9, 
   3.5245884173153324`*^9, 3.525090305225604*^9, 3.525091712931485*^9, 
   3.52509196976824*^9, 3.5250922081188297`*^9, 3.525092463638977*^9, 
   3.525092583617162*^9, 3.525092707747345*^9, 3.525092911489631*^9, 
   3.525093231117325*^9, 3.5250987903912973`*^9, 3.525448011868248*^9, 
   3.5254612341573496`*^9, 3.5255182937570796`*^9, 3.525824603157239*^9}],

Cell[BoxData[
 RowBox[{"{", 
  RowBox[{"{", 
   RowBox[{
    RowBox[{
     FractionBox["1", "250000"], 
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"-", "685"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "]"}]}], "-", 
        RowBox[{"514", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "+", 
        RowBox[{"514", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
        RowBox[{"685", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
       ")"}]}]}], ",", 
    RowBox[{
     FractionBox["1", "250000"], 
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"-", "514"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}]}], "+", 
        RowBox[{"514", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
        RowBox[{"685", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
       ")"}]}]}], ",", 
    RowBox[{
     FractionBox["1", "250000"], 
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{"514", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "+", 
        RowBox[{"685", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
       ")"}]}]}], ",", 
    RowBox[{
     FractionBox["1", "250000"], 
     RowBox[{"127", " ", 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"-", "514"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "]"}]}], "-", 
        RowBox[{"685", " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}]}], 
       ")"}]}]}], ",", 
    RowBox[{"-", 
     FractionBox[
      RowBox[{"17399", " ", 
       RowBox[{"Sin", "[", 
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
         RowBox[{
          SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "-", 
         RowBox[{
          SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}]}], 
      "50000"]}]}], "}"}], "}"}]], "Output",
 CellChangeTimes->{{3.5158956386727686`*^9, 3.5158956398638363`*^9}, {
   3.5158958866799536`*^9, 3.515895900344735*^9}, 3.515896802422331*^9, 
   3.515897627033496*^9, 3.5159462523356647`*^9, 3.515985921767785*^9, 
   3.516707287973062*^9, 3.5167100467628555`*^9, 3.5167114899284*^9, 
   3.516712848515107*^9, 3.5167321046199136`*^9, 3.5167517629159827`*^9, 
   3.517679445919633*^9, 3.5176799540956993`*^9, 3.517680290765956*^9, 
   3.517686202845107*^9, 3.517698252865329*^9, 3.517698806692006*^9, 
   3.520728549140354*^9, 3.5208973266942844`*^9, 3.521304022935862*^9, 
   3.521486069771743*^9, 3.521487936998542*^9, 3.52224513490092*^9, 
   3.522246821400382*^9, 3.5222499247016582`*^9, 3.522330741871931*^9, 
   3.5223566332623515`*^9, 3.522356937710122*^9, 3.5224296785949993`*^9, 
   3.5225106422336226`*^9, 3.5225114183960166`*^9, 3.522511902505706*^9, 
   3.522708167077591*^9, 3.52277517434692*^9, 3.5227768699202423`*^9, 
   3.523028613473358*^9, 3.5230313612175198`*^9, 3.523557292400724*^9, 
   3.5237248928757167`*^9, 3.5237265842264566`*^9, 3.5239837637362833`*^9, 
   3.523985011164632*^9, 3.523989325828417*^9, 3.523990349878989*^9, 
   3.523992043299847*^9, 3.5239921551712456`*^9, 3.523994836828628*^9, 
   3.523994946803918*^9, 3.523995057892272*^9, 3.524248160081889*^9, 
   3.5242485466930017`*^9, 3.5242500287246056`*^9, 3.5244001564861712`*^9, 
   3.5244003296869793`*^9, 3.524406307005149*^9, 3.524406577435484*^9, 
   3.524406806634446*^9, 3.524418419968019*^9, 3.5244187277574553`*^9, 
   3.524422251980219*^9, 3.5244223391402044`*^9, 3.524422778897357*^9, 
   3.524423869471734*^9, 3.5244422436216755`*^9, 3.524442769671764*^9, 
   3.5244438766880817`*^9, 3.5244457141591787`*^9, 3.5244458917423363`*^9, 
   3.524446905993886*^9, 3.524502459654175*^9, 3.5245119778979416`*^9, 
   3.5245884173153324`*^9, 3.525090305225604*^9, 3.525091712931485*^9, 
   3.52509196976824*^9, 3.5250922081188297`*^9, 3.525092463638977*^9, 
   3.525092583617162*^9, 3.525092707747345*^9, 3.525092911489631*^9, 
   3.525093231117325*^9, 3.5250987903912973`*^9, 3.525448011868248*^9, 
   3.5254612341573496`*^9, 3.5255182937570796`*^9, 3.5258246031602387`*^9}]
}, Open  ]],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<h_dot_minus\>\"", "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Dh", "/.", "statesubs"}]}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158874776569853`*^9, 3.5158874849574027`*^9}, {
  3.515887517663273*^9, 3.5158875400105515`*^9}, {3.515887575902604*^9, 
  3.5158875781407323`*^9}, {3.515895688195601*^9, 3.515895688494618*^9}, {
  3.51589577315246*^9, 3.515895776011624*^9}, {3.515895808711494*^9, 
  3.515895808919506*^9}, {3.5158958389342227`*^9, 3.515895841410364*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{"NotebookDirectory", "[", "]"}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{"Run", "[", "\"\<perl math2matopt_torso2.pl\>\"", 
  "]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{
    RowBox[{"NotebookDirectory", "[", "]"}], "<>", "\"\<buildopt_torso\>\""}],
    "]"}], ";"}]}], "Input",
 CellChangeTimes->{
  3.515897431926337*^9, {3.5158975074906588`*^9, 3.5158975156601257`*^9}, 
   3.515946262132225*^9, 3.5159464148519597`*^9, 3.5207285400618353`*^9}],

Cell[BoxData["0"], "Output",
 CellChangeTimes->{
  3.5158942533265314`*^9, 3.5158942880935197`*^9, 3.5158943288598514`*^9, 
   3.515894540344948*^9, 3.515894730479823*^9, 3.5158951421403685`*^9, {
   3.515895197692546*^9, 3.515895216536624*^9}, 3.5158957825950003`*^9, 
   3.5158958472696996`*^9, {3.5158958868709645`*^9, 3.5158959006497526`*^9}, 
   3.5158968026583443`*^9, 3.5158976273045115`*^9, 3.515899002372161*^9, 
   3.515899444381442*^9, 3.515899516217551*^9, 3.515946252575678*^9, 
   3.5159463809820223`*^9, 3.5159464201432624`*^9, 3.5159466873265443`*^9, 
   3.5159582714411182`*^9, 3.5159588013434267`*^9, 3.5159632640938406`*^9, 
   3.5159635466520014`*^9, 3.51597316587319*^9, 3.5159737776421804`*^9, 
   3.5159738712395344`*^9, 3.5159859224928265`*^9, 3.5167072881200705`*^9, 
   3.5167100469328656`*^9, 3.5167114900894094`*^9, 3.5167122547441454`*^9, 
   3.5167128486161127`*^9, 3.516732104761922*^9, 3.516751763054991*^9, 
   3.5176794461376457`*^9, 3.517679954403717*^9, 3.517680290953966*^9, 
   3.5176862030151167`*^9, 3.517698252965335*^9, 3.5176988068760166`*^9, 
   3.5207285492783623`*^9, 3.5208973270173025`*^9, 3.5213040231208725`*^9, 
   3.5214860699177513`*^9, 3.521487937090547*^9, 3.522245135005926*^9, 
   3.52224682153039*^9, 3.522249924857668*^9, 3.5223307420439415`*^9, 
   3.522356633418352*^9, 3.522356937803723*^9, 3.522429678706005*^9, 
   3.5225106423946323`*^9, 3.522511169265767*^9, 3.522511418613029*^9, 
   3.5225119026857166`*^9, 3.5227081672426004`*^9, 3.522775174405923*^9, 
   3.5227768700482492`*^9, 3.5230286136163664`*^9, 3.5230313613095255`*^9, 
   3.5235572925177307`*^9, 3.5237248929757223`*^9, 3.523726584300461*^9, 
   3.5239837637952867`*^9, 3.5239850112496367`*^9, 3.523989325897421*^9, 
   3.523990349944993*^9, 3.523992043393852*^9, 3.52399215524125*^9, 
   3.5239948369206333`*^9, 3.5239949468979235`*^9, 3.5239950579642763`*^9, 
   3.5242481601638937`*^9, 3.524248546765006*^9, 3.5242500336108847`*^9, 
   3.524400156546171*^9, 3.5244003298319883`*^9, 3.5244063070871515`*^9, 
   3.5244065775044875`*^9, 3.52440680670545*^9, 3.5244184200480194`*^9, 
   3.52441872783646*^9, 3.5244222520602236`*^9, 3.5244223391972075`*^9, 
   3.5244227789383593`*^9, 3.5244238695467386`*^9, 3.5244422436906796`*^9, 
   3.524442769744768*^9, 3.5244438767490854`*^9, 3.5244457142201824`*^9, 
   3.5244458918003397`*^9, 3.5244469061228933`*^9, 3.5245024598101835`*^9, 
   3.5245119780499506`*^9, 3.5245884173863363`*^9, 3.5250903053076086`*^9, 
   3.525091712991485*^9, 3.525091969842244*^9, 3.525092208180833*^9, 
   3.5250924637039804`*^9, 3.5250925836771655`*^9, 3.5250927078223495`*^9, 
   3.525092911592637*^9, 3.525093231243332*^9, 3.525098790547306*^9, 
   3.525448012031257*^9, 3.5254612342233534`*^9, 3.525518293902088*^9, 
   3.525824603411253*^9}]
}, Open  ]],

Cell[BoxData[
 RowBox[{
  RowBox[{"tf", "=", 
   RowBox[{"AbsoluteTime", "[", "]"}]}], ";"}]], "Input",
 CellChangeTimes->{{3.522246710599045*^9, 3.5222467142022505`*^9}}],

Cell["Computatin time in seconds", "Text",
 CellChangeTimes->{{3.5222467382666273`*^9, 3.522246741185794*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"tf", "-", "t0"}]], "Input",
 CellChangeTimes->{{3.522246715815343*^9, 3.522246742443866*^9}}],

Cell[BoxData["136.3718`9.58626956637818"], "Output",
 CellChangeTimes->CompressedData["
1:eJwV0F0oqwEcx/HHSTlFonRORxHyMm956USM2bNhK29raFt5KTkh9diFJs8k
bVzKcfYUlraURlqLrSgXaChFS7l4KLmSw1N0lJeNk/z+F78+d7+Lb3rPkP5X
FMMwNdiB05+Uo5DYYKVrXgZt48F4HdT3FAU6oGZLfkH++CJ2TsDfQvPNCnzU
tGtW4U+HaCd10q3xa43EKjNDCbnQUr1/TXpTtk6X4ZU5X+aBKvmfZ49SYi8L
lo7DkCmPdUfg9ixfOsJKrON2IES2yXbuRqFJvjvAQ0nkqq3Q/Le/gcyyuZ1j
UEzsXiX1PuUmOZcY0xiG0WemMdItlJdFYBU/a1ep8JMnXyD3B3sn6yDjK/GQ
U+9ikIxk2LkGuFfwXyAnj+xNTVDN9RnJV2PXDBlIC3xvhopXS4IBPlkq9OTa
x/wl+S1ZGzRCk5M/J8sMdQYTzH60jgxDPqROGYXrH4VmF/znzZtwqCX2ojW9
WIDxviodGVcZw5HRJeFpMnXpZZHceLN6SUHxdEjWP9zdk/6us+U5qPbbclW1
6J6vdWnhSeEO3wulFi72DX4C70P0+A==
  "]]
}, Open  ]],

Cell["\<\
Times on various platforms (add yours in):\
\>", "Text",
 CellChangeTimes->{{3.5222468263546658`*^9, 3.522246844858724*^9}, {
  3.5222469044081297`*^9, 3.5222469067202625`*^9}}],

Cell["\<\
980x Gulftown, SSD \[Rule] 43.57 seconds\
\>", "Item",
 CellChangeTimes->{{3.522246852675171*^9, 3.522246891440388*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell["Write Outputs To disk", "Section",
 CellChangeTimes->{{3.527176889623287*^9, 3.5271768951406612`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<jpos_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"pos", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<D_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"\[ScriptCapitalD]", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<C_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"\[ScriptCapitalC]", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<G_vec\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"\[ScriptCapitalG]", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<De_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"\[ScriptCapitalD]e", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<E_mat\>\"", "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"\[ScriptCapitalE]", "/.", "statesubs"}]}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<h_sca\>\"", "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"h", "/.", "statesubs"}]}], "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<h_dot_mat\>\"", "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"hdot", "/.", "statesubs"}]}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input"],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<nstorsoFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"nstorso", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.52717692204714*^9, 3.5271769389718723`*^9}, {
  3.527177770383316*^9, 3.5271777843116817`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<deltaNStorsoFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"LinearNStorso", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5271773510313377`*^9, 3.527177374463675*^9}, 
   3.527177789505682*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<storsoFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"storso", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.52717749547878*^9, 3.527177496241226*^9}, {
   3.527177526742309*^9, 3.52717753978264*^9}, {3.527177650458077*^9, 
   3.527177650554824*^9}, 3.527177794059635*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<deltaStorsoFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"LinearStorso", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{3.527177499009471*^9, 3.527177543096356*^9, 
  3.5271777958436832`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<hipposFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"HipPos", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.527177507189144*^9, 3.5271775544415283`*^9}, 
   3.5271777971433496`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<deltaHipposFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"LHipPos", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.527177561917758*^9, 3.527177576221059*^9}, 
   3.527177798533535*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<nsslopeFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"nsslope", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"Clear", "[", "stream", "]"}], ";"}], 
  "\[IndentingNewLine]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<deltaNSslopeFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{"LinearNSslope", " ", "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.527177585551565*^9, 3.5271776182148*^9}, {
  3.527177799901759*^9, 3.5271778041575823`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<comFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["p", "COM"], "/.", "p0"}], "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"Clear", "[", "stream", "]"}], ";"}], 
  "\[IndentingNewLine]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<deltaCOMFn\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["Linearp", "COM"], " ", "/.", "p0"}], "/.", 
      "\[InvisibleSpace]", "constsubs"}], "/.", "statesubs"}]}], "]"}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.527178155956102*^9, 3.527178183671091*^9}, {
  3.52717873969038*^9, 3.527178747613838*^9}}],

Cell[CellGroupData[{

Cell["PERL", "Subsection",
 CellChangeTimes->{{3.51353232609383*^9, 3.513532329542585*^9}}],

Cell["this next line assumes that perl is in your path", "Text",
 CellChangeTimes->{{3.513015230408543*^9, 3.513015241200741*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{"NotebookDirectory", "[", "]"}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{"Run", "[", "\"\<perl math2mat_output.pl\>\"", 
  "]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{
    RowBox[{"NotebookDirectory", "[", "]"}], "<>", "\"\<build_torso\>\""}], 
   "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.504476276692404*^9, 3.504476301348216*^9}, {
   3.504478868125564*^9, 3.504478869967337*^9}, {3.504538702237328*^9, 
   3.504538704315194*^9}, {3.504631487786531*^9, 3.504631500259924*^9}, {
   3.504632214564033*^9, 3.504632215194798*^9}, {3.504635416937057*^9, 
   3.50463541912094*^9}, {3.504874639115547*^9, 3.504874639679282*^9}, 
   3.504876166859266*^9, {3.505578303460942*^9, 3.5055783039289427`*^9}, {
   3.512922890945364*^9, 3.512922891900902*^9}, {3.5129244660544987`*^9, 
   3.51292449464812*^9}, {3.512925468730114*^9, 3.512925469241001*^9}, {
   3.513011661780643*^9, 3.513011661926162*^9}, {3.51301497676734*^9, 
   3.513015001300451*^9}, {3.5158700422275343`*^9, 3.515870043613613*^9}, {
   3.515870412159693*^9, 3.5158704163519325`*^9}, {3.5158863672774124`*^9, 
   3.5158863722382207`*^9}, {3.5271773836362467`*^9, 3.527177386040779*^9}}],

Cell[BoxData[
 FormBox["0", TraditionalForm]], "Output",
 CellChangeTimes->{
  3.5155188337118206`*^9, 3.5155213252703295`*^9, {3.515540878198961*^9, 
   3.515540903604343*^9}, 3.5158407423966546`*^9, 3.5158416340670977`*^9, 
   3.5158498815118246`*^9, 3.515850149364145*^9, 3.5158505826739287`*^9, 
   3.5158508174343567`*^9, 3.5158510975593786`*^9, 3.515851653032055*^9, 
   3.5158525885305624`*^9, 3.5158527509368515`*^9, 3.5158527931192636`*^9, 
   3.51585495715904*^9, 3.5158550282321053`*^9, 3.5158552191920276`*^9, 
   3.515855253120968*^9, 3.515865262254135*^9, 3.515865638210639*^9, 
   3.51586592793721*^9, 3.51587048063961*^9, 3.5158730153755884`*^9, 
   3.5158739792467184`*^9, 3.515874203634553*^9, 3.515886694531988*^9, 
   3.5158874913514147`*^9, 3.515889070384316*^9, 3.5158900577352266`*^9, 
   3.515890252205177*^9, 3.5159482968094587`*^9, 3.515948944374403*^9, 
   3.515953395315056*^9, 3.515953710496083*^9, 3.5159537761788397`*^9, 
   3.5159726585821743`*^9, 3.5159857720472217`*^9, 3.516707085463479*^9, 
   3.516709802924909*^9, 3.5167112898219547`*^9, 3.5167126251093287`*^9, 
   3.516731910384804*^9, 3.51675156425562*^9, 3.517679232673436*^9, 
   3.517679752897191*^9, 3.517680087541332*^9, 3.5176859974273577`*^9, 
   3.5176980466975365`*^9, 3.5176984843215675`*^9, 3.517698597905064*^9, 
   3.520109553737687*^9, 3.520120022112444*^9, 3.520177851995036*^9, 
   3.5201861307705555`*^9, 3.520256997752266*^9, 3.5202590894559045`*^9, 
   3.520339215071435*^9, 3.5204373594555745`*^9, 3.5204379012015605`*^9, 
   3.5204436639822655`*^9, 3.520449216190834*^9, 3.520456964627019*^9, 
   3.520532857654432*^9, 3.5205334050077386`*^9, 3.520533641289253*^9, 
   3.520700208631287*^9, 3.520713025182435*^9, 3.520715993050187*^9, 
   3.520716372698902*^9, 3.5207164419848647`*^9, 3.520720498096861*^9, 
   3.520734731372958*^9, 3.5207350240366974`*^9, 3.520735233941703*^9, 
   3.5207876835786457`*^9, 3.520788877843954*^9, 3.5208972402433395`*^9, 
   3.5213037296170855`*^9, 3.5213038482928734`*^9, 3.52148604821651*^9, 
   3.521487916633377*^9, 3.5222451196790495`*^9, 3.522246794946869*^9, 
   3.52224990060228*^9, 3.522330715643431*^9, 3.5223566127951155`*^9, 
   3.5223569178824873`*^9, 3.5224296564487324`*^9, 3.522510361860586*^9, 
   3.5225118733730397`*^9, 3.522708144818318*^9, 3.5227751575219574`*^9, 
   3.522776845738859*^9, 3.5230285840646763`*^9, 3.5230306390121126`*^9, 
   3.523031339818296*^9, 3.5235572703774643`*^9, 3.52372487579274*^9, 
   3.5237265671694813`*^9, 3.5239837459472656`*^9, 3.5239849930135937`*^9, 
   3.5239893079543943`*^9, 3.5239903317719536`*^9, 3.523992025216813*^9, 
   3.5239921368791995`*^9, 3.5239948186215863`*^9, 3.5239949284198666`*^9, 
   3.5239950392682066`*^9, 3.5242481412858143`*^9, 3.524248528452959*^9, 
   3.5242499977068315`*^9, 3.5244001128388147`*^9, 3.5244002976151495`*^9, 
   3.5244062695321836`*^9, 3.524406550370155*^9, 3.5244067850253863`*^9, 
   3.524416267410578*^9, 3.524417771154109*^9, 3.5244178686872463`*^9, 
   3.5244183936479816`*^9, 3.524418700642165*^9, 3.524422224071623*^9, 
   3.5244223112816105`*^9, 3.524422752104824*^9, 3.5244238419211583`*^9, 
   3.524442216849144*^9, 3.524442742387203*^9, 3.5244438493725195`*^9, 
   3.5244456864925966`*^9, 3.5244458645247793`*^9, 3.5244468702188396`*^9, 
   3.5245024335606823`*^9, 3.5245119471381826`*^9, 3.5245883990742893`*^9, 
   3.525090287048571*^9, 3.525091695188668*^9, 3.5250919519797196`*^9, 
   3.5250921900848627`*^9, 3.5250924453299904`*^9, 3.525092571060454*^9, 
   3.52509268956332*^9, 3.5250928935899105`*^9, 3.5250932069259415`*^9, 
   3.525098766927955*^9, 3.5254479897919855`*^9, 3.525461216302328*^9, 
   3.5255182690806684`*^9, 3.525824548871134*^9, 3.527177429714081*^9, 
   3.527177710324808*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell["write the control files to disk", "Subsubsection",
 CellChangeTimes->{{3.513533556726437*^9, 3.513533560633779*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<sigma_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{"\[Sigma]y", "/.", "\[InvisibleSpace]", "constsubs"}], "/.", 
     "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533598891416*^9}, {
   3.513534630912414*^9, 3.513534631668521*^9}, {3.513534662555744*^9, 
   3.513534664379022*^9}, {3.513535683786766*^9, 3.513535684907331*^9}, {
   3.513535919636849*^9, 3.513535976164574*^9}, {3.513873084387678*^9, 
   3.513873093721922*^9}, {3.515862148936386*^9, 3.5158621546717143`*^9}, {
   3.516708335792994*^9, 3.516708336128013*^9}, 3.5207161434147873`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<phip_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["p", "hip"], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533598891416*^9}, {
   3.513534630912414*^9, 3.513534631668521*^9}, {3.513534662555744*^9, 
   3.513534664379022*^9}, {3.513535683786766*^9, 3.513535684907331*^9}, {
   3.513535919636849*^9, 3.513535976164574*^9}, {3.513873084387678*^9, 
   3.513873093721922*^9}, {3.5158698657024374`*^9, 3.5158698742209244`*^9}, {
   3.516708336825053*^9, 3.516708337152072*^9}, 3.520716145044881*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<deltaphip_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["deltap", "hip"], "/.", "\[InvisibleSpace]", "constsubs"}],
      "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5201856101017747`*^9, 3.5201856221114616`*^9}, 
   3.52071614660497*^9, {3.5207164303892016`*^9, 3.5207164345454397`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<phip_dot_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["p", "hipdot"], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533598891416*^9}, {
   3.513534630912414*^9, 3.513534631668521*^9}, {3.513534662555744*^9, 
   3.513534664379022*^9}, {3.513535683786766*^9, 3.513535684907331*^9}, {
   3.513535919636849*^9, 3.513535976164574*^9}, {3.513873084387678*^9, 
   3.513873093721922*^9}, {3.5158698657024374`*^9, 3.5158698742209244`*^9}, {
   3.516708336825053*^9, 3.516708337152072*^9}, {3.5167541031348357`*^9, 
   3.516754114654495*^9}, 3.520716147794038*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<deltaphip_dot_mat\>\"", "]"}]}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["deltap", "hipdot"], " ", "/.", "\[InvisibleSpace]", 
      "constsubs"}], "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533598891416*^9}, {
  3.513534630912414*^9, 3.513534631668521*^9}, {3.513534662555744*^9, 
  3.513534664379022*^9}, {3.513535683786766*^9, 3.513535684907331*^9}, {
  3.513535919636849*^9, 3.513535976164574*^9}, {3.513873084387678*^9, 
  3.513873093721922*^9}, {3.5158698657024374`*^9, 3.5158698742209244`*^9}, {
  3.516708336825053*^9, 3.516708337152072*^9}, {3.5167541031348357`*^9, 
  3.516754114654495*^9}, {3.5203390769255333`*^9, 3.520339105876189*^9}, {
  3.520716149641144*^9, 3.5207161499701624`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<pcom_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["p", "COM"], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158901103775215`*^9, 3.5158901154943304`*^9}, {
   3.515948100165904*^9, 3.515948106905116*^9}, {3.5159487206466036`*^9, 
   3.5159487213174047`*^9}, {3.51594878707152*^9, 3.5159487941383324`*^9}, {
   3.515953230430625*^9, 3.51595323192271*^9}, {3.515953761686011*^9, 
   3.515953766147266*^9}, 3.515985484570779*^9, {3.5167083379371166`*^9, 
   3.5167083382401342`*^9}, {3.5207162236953793`*^9, 3.5207162275425997`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<yd1_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["y", 
       RowBox[{"d", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158726672456765`*^9, 3.51587268496169*^9}, {
   3.5167083389361734`*^9, 3.516708341929345*^9}, 3.5207162296587205`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<yd2_vec\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["y", 
       RowBox[{"d", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533598891416*^9}, 
   3.513535978245256*^9, {3.5139480491379*^9, 3.513948080396936*^9}, {
   3.513948113545785*^9, 3.513948129942129*^9}, 3.5158619633557715`*^9, {
   3.5158726892259336`*^9, 3.515872692325111*^9}, {3.5167083517449064`*^9, 
   3.5167083521129274`*^9}, 3.5207162311398053`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Dyd1_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["Dy", 
       RowBox[{"d", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158727027727084`*^9, 3.515872707855999*^9}, {
   3.5167083527929664`*^9, 3.5167083530889835`*^9}, 3.5207162325458856`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Dyd2_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["Dy", 
       RowBox[{"d", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533604687178*^9}, {
   3.513535690806333*^9, 3.513535693339207*^9}, 3.513535980161328*^9, 
   3.51586196000458*^9, {3.515872712110243*^9, 3.5158727149254036`*^9}, {
   3.516708353801024*^9, 3.5167083540880404`*^9}, 3.5207163273033056`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<DLfyd1_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["DLfy", 
       RowBox[{"d", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.515861942956605*^9, 3.515861955946348*^9}, {
   3.515874032033738*^9, 3.515874045405503*^9}, {3.5167083552411065`*^9, 
   3.5167083555441236`*^9}, 3.520716332000574*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<DLfyd2_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["DLfy", 
       RowBox[{"d", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.515861942956605*^9, 3.515861955946348*^9}, {
   3.5158740329057875`*^9, 3.515874042230321*^9}, {3.5167083567361917`*^9, 
   3.5167083570722113`*^9}, 3.5207163337346735`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<ya1_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["y", 
       RowBox[{"a", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158727241079288`*^9, 3.5158727304642925`*^9}, {
   3.516708359905373*^9, 3.5167083605204086`*^9}, 3.52071633560478*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<ya2_vec\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["y", 
       RowBox[{"a", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533598891416*^9}, {
   3.513535713498742*^9, 3.513535727403304*^9}, {3.5158727332724533`*^9, 
   3.5158727361286163`*^9}, {3.5167083610644393`*^9, 3.516708361376457*^9}, 
   3.5207163401990433`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Dya1_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["Dy", 
       RowBox[{"a", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158727417019353`*^9, 3.51587274563416*^9}, {
   3.5167083619924927`*^9, 3.5167083623125105`*^9}, 3.520716341684128*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Dya2_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["Dy", 
       RowBox[{"a", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533604687178*^9}, {
   3.513535690806333*^9, 3.51353572494454*^9}, {3.515872742920005*^9, 
   3.5158727481833057`*^9}, {3.5167083632325635`*^9, 3.5167083635285807`*^9}, 
   3.5207163448703103`*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<DLfya1_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["DLfy", 
       RowBox[{"a", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.513533562008006*^9, 3.513533607692186*^9}, 
   3.513534673091028*^9, {3.513535695836862*^9, 3.513535722564655*^9}, {
   3.513536332459772*^9, 3.513536336316357*^9}, {3.5158727571118164`*^9, 
   3.515872763041156*^9}, {3.516708364688647*^9, 3.516708364976663*^9}, 
   3.520716347555464*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<DLfya2_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["DLfy", 
       RowBox[{"a", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.5158727601249886`*^9, 3.5158727645362415`*^9}, {
   3.516708366824769*^9, 3.516708367128786*^9}, 3.520716352285734*^9}]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell["PERL", "Subsection",
 CellChangeTimes->{{3.51353232609383*^9, 3.513532329542585*^9}}],

Cell["this next line assumes that perl is in your path", "Text",
 CellChangeTimes->{{3.513015230408543*^9, 3.513015241200741*^9}}],

Cell[CellGroupData[{

Cell[BoxData[{
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{"NotebookDirectory", "[", "]"}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{"Run", "[", "\"\<perl math2mat_torso.pl\>\"", 
  "]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{
    RowBox[{"NotebookDirectory", "[", "]"}], "<>", "\"\<build_torso\>\""}], 
   "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.504476276692404*^9, 3.504476301348216*^9}, {
   3.504478868125564*^9, 3.504478869967337*^9}, {3.504538702237328*^9, 
   3.504538704315194*^9}, {3.504631487786531*^9, 3.504631500259924*^9}, {
   3.504632214564033*^9, 3.504632215194798*^9}, {3.504635416937057*^9, 
   3.50463541912094*^9}, {3.504874639115547*^9, 3.504874639679282*^9}, 
   3.504876166859266*^9, {3.505578303460942*^9, 3.5055783039289427`*^9}, {
   3.512922890945364*^9, 3.512922891900902*^9}, {3.5129244660544987`*^9, 
   3.51292449464812*^9}, {3.512925468730114*^9, 3.512925469241001*^9}, {
   3.513011661780643*^9, 3.513011661926162*^9}, {3.51301497676734*^9, 
   3.513015001300451*^9}, {3.5158700422275343`*^9, 3.515870043613613*^9}, {
   3.515870412159693*^9, 3.5158704163519325`*^9}, {3.5158863672774124`*^9, 
   3.5158863722382207`*^9}}],

Cell[BoxData["0"], "Output",
 CellChangeTimes->{
  3.5155188337118206`*^9, 3.5155213252703295`*^9, {3.515540878198961*^9, 
   3.515540903604343*^9}, 3.5158407423966546`*^9, 3.5158416340670977`*^9, 
   3.5158498815118246`*^9, 3.515850149364145*^9, 3.5158505826739287`*^9, 
   3.5158508174343567`*^9, 3.5158510975593786`*^9, 3.515851653032055*^9, 
   3.5158525885305624`*^9, 3.5158527509368515`*^9, 3.5158527931192636`*^9, 
   3.51585495715904*^9, 3.5158550282321053`*^9, 3.5158552191920276`*^9, 
   3.515855253120968*^9, 3.515865262254135*^9, 3.515865638210639*^9, 
   3.51586592793721*^9, 3.51587048063961*^9, 3.5158730153755884`*^9, 
   3.5158739792467184`*^9, 3.515874203634553*^9, 3.515886694531988*^9, 
   3.5158874913514147`*^9, 3.515889070384316*^9, 3.5158900577352266`*^9, 
   3.515890252205177*^9, 3.5159482968094587`*^9, 3.515948944374403*^9, 
   3.515953395315056*^9, 3.515953710496083*^9, 3.5159537761788397`*^9, 
   3.5159726585821743`*^9, 3.5159857720472217`*^9, 3.516707085463479*^9, 
   3.516709802924909*^9, 3.5167112898219547`*^9, 3.5167126251093287`*^9, 
   3.516731910384804*^9, 3.51675156425562*^9, 3.517679232673436*^9, 
   3.517679752897191*^9, 3.517680087541332*^9, 3.5176859974273577`*^9, 
   3.5176980466975365`*^9, 3.5176984843215675`*^9, 3.517698597905064*^9, 
   3.520109553737687*^9, 3.520120022112444*^9, 3.520177851995036*^9, 
   3.5201861307705555`*^9, 3.520256997752266*^9, 3.5202590894559045`*^9, 
   3.520339215071435*^9, 3.5204373594555745`*^9, 3.5204379012015605`*^9, 
   3.5204436639822655`*^9, 3.520449216190834*^9, 3.520456964627019*^9, 
   3.520532857654432*^9, 3.5205334050077386`*^9, 3.520533641289253*^9, 
   3.520700208631287*^9, 3.520713025182435*^9, 3.520715993050187*^9, 
   3.520716372698902*^9, 3.5207164419848647`*^9, 3.520720498096861*^9, 
   3.520734731372958*^9, 3.5207350240366974`*^9, 3.520735233941703*^9, 
   3.5207876835786457`*^9, 3.520788877843954*^9, 3.5208972402433395`*^9, 
   3.5213037296170855`*^9, 3.5213038482928734`*^9, 3.52148604821651*^9, 
   3.521487916633377*^9, 3.5222451196790495`*^9, 3.522246794946869*^9, 
   3.52224990060228*^9, 3.522330715643431*^9, 3.5223566127951155`*^9, 
   3.5223569178824873`*^9, 3.5224296564487324`*^9, 3.522510361860586*^9, 
   3.5225118733730397`*^9, 3.522708144818318*^9, 3.5227751575219574`*^9, 
   3.522776845738859*^9, 3.5230285840646763`*^9, 3.5230306390121126`*^9, 
   3.523031339818296*^9, 3.5235572703774643`*^9, 3.52372487579274*^9, 
   3.5237265671694813`*^9, 3.5239837459472656`*^9, 3.5239849930135937`*^9, 
   3.5239893079543943`*^9, 3.5239903317719536`*^9, 3.523992025216813*^9, 
   3.5239921368791995`*^9, 3.5239948186215863`*^9, 3.5239949284198666`*^9, 
   3.5239950392682066`*^9, 3.5242481412858143`*^9, 3.524248528452959*^9, 
   3.5242499977068315`*^9, 3.5244001128388147`*^9, 3.5244002976151495`*^9, 
   3.5244062695321836`*^9, 3.524406550370155*^9, 3.5244067850253863`*^9, 
   3.524416267410578*^9, 3.524417771154109*^9, 3.5244178686872463`*^9, 
   3.5244183936479816`*^9, 3.524418700642165*^9, 3.524422224071623*^9, 
   3.5244223112816105`*^9, 3.524422752104824*^9, 3.5244238419211583`*^9, 
   3.524442216849144*^9, 3.524442742387203*^9, 3.5244438493725195`*^9, 
   3.5244456864925966`*^9, 3.5244458645247793`*^9, 3.5244468702188396`*^9, 
   3.5245024335606823`*^9, 3.5245119471381826`*^9, 3.5245883990742893`*^9, 
   3.525090287048571*^9, 3.525091695188668*^9, 3.5250919519797196`*^9, 
   3.5250921900848627`*^9, 3.5250924453299904`*^9, 3.525092571060454*^9, 
   3.52509268956332*^9, 3.5250928935899105`*^9, 3.5250932069259415`*^9, 
   3.525098766927955*^9, 3.5254479897919855`*^9, 3.525461216302328*^9, 
   3.5255182690806684`*^9, 3.525824548871134*^9}]
}, Open  ]]
}, Open  ]]
}, Open  ]]
},
WindowSize->{959, 1026},
WindowMargins->{{Automatic, -10}, {Automatic, -8}},
ShowSelection->True,
Magnification->1.100000023841858,
FrontEndVersion->"8.0 for Linux x86 (64-bit) (November 7, 2010)",
StyleDefinitions->"Default.nb"
]
(* End of Notebook Content *)

(* Internal cache information *)
(*CellTagsOutline
CellTagsIndex->{}
*)
(*CellTagsIndex
CellTagsIndex->{}
*)
(*NotebookFileOutline
Notebook[{
Cell[545, 20, 1084, 24, 108, "Input"],
Cell[1632, 46, 2708, 62, 184, "Input"],
Cell[4343, 110, 7525, 219, 575, "Input"],
Cell[11871, 331, 33970, 993, 1615, "Input"],
Cell[45844, 1326, 15411, 438, 1032, "Input"],
Cell[61258, 1766, 9046, 277, 653, "Input"],
Cell[70307, 2045, 2227, 64, 185, "Input"],
Cell[72537, 2111, 3416, 106, 181, "Input"],
Cell[75956, 2219, 4494, 111, 188, "Input"],
Cell[80453, 2332, 26, 0, 31, "Input"],
Cell[80482, 2334, 13443, 362, 1571, "Input"],
Cell[93928, 2698, 499, 11, 51, "Input"],
Cell[94430, 2711, 102, 1, 30, "Text"],
Cell[94535, 2714, 1163, 26, 108, "Input"],
Cell[95701, 2742, 238, 3, 30, "Text"],
Cell[95942, 2747, 302, 7, 31, "Input"],
Cell[96247, 2756, 174, 2, 30, "Text"],
Cell[CellGroupData[{
Cell[96446, 2762, 796, 22, 89, "Input"],
Cell[97245, 2786, 3813, 73, 31, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[101095, 2864, 184, 2, 40, "Subsection"],
Cell[CellGroupData[{
Cell[101304, 2870, 1098, 26, 53, "Input"],
Cell[102405, 2898, 3458, 59, 48, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[105900, 2962, 34, 0, 31, "Input"],
Cell[105937, 2964, 1949, 50, 31, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[107923, 3019, 186, 3, 31, "Input"],
Cell[108112, 3024, 1962, 48, 77, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[110111, 3077, 110, 3, 31, "Input"],
Cell[110224, 3082, 1427, 36, 61, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[111688, 3123, 687, 16, 51, "Input"],
Cell[112378, 3141, 4317, 85, 71, "Output"]
}, Open  ]],
Cell[116710, 3229, 444, 12, 89, "Input"],
Cell[117157, 3243, 137, 1, 30, "Text"],
Cell[CellGroupData[{
Cell[117319, 3248, 465, 13, 31, "Input"],
Cell[117787, 3263, 26940, 695, 900, "Output"]
}, Open  ]],
Cell[144742, 3961, 136, 1, 30, "Text"],
Cell[CellGroupData[{
Cell[144903, 3966, 301, 8, 31, "Input"],
Cell[145207, 3976, 4344, 98, 82, "Output"]
}, Open  ]],
Cell[149566, 4077, 514, 14, 108, "Input"],
Cell[CellGroupData[{
Cell[150105, 4095, 274, 6, 31, "Input"],
Cell[150382, 4103, 35414, 866, 986, "Output"]
}, Open  ]],
Cell[185811, 4972, 477, 13, 108, "Input"],
Cell[CellGroupData[{
Cell[186313, 4989, 327, 4, 29, "Subsubsection"],
Cell[186643, 4995, 131, 1, 30, "Text"],
Cell[186777, 4998, 2315, 59, 222, "Input"],
Cell[189095, 5059, 3384, 99, 165, "Input"],
Cell[192482, 5160, 418, 12, 31, "Input"],
Cell[CellGroupData[{
Cell[192925, 5176, 124, 2, 31, "Input"],
Cell[193052, 5180, 1895, 44, 93, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[194984, 5229, 716, 19, 51, "Input"],
Cell[195703, 5250, 4919, 132, 123, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[200659, 5387, 336, 7, 31, "Input"],
Cell[200998, 5396, 6598, 151, 123, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[207633, 5552, 4126, 104, 127, "Input"],
Cell[211762, 5658, 2839, 42, 31, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[214638, 5705, 1301, 34, 108, "Input"],
Cell[215942, 5741, 5544, 149, 78, "Output"],
Cell[221489, 5892, 934, 20, 31, "Output"],
Cell[222426, 5914, 1070, 24, 31, "Output"],
Cell[223499, 5940, 8467, 225, 115, "Output"],
Cell[231969, 6167, 934, 20, 31, "Output"]
}, Open  ]],
Cell[232918, 6190, 608, 15, 89, "Input"],
Cell[233529, 6207, 632, 15, 89, "Input"],
Cell[234164, 6224, 632, 15, 89, "Input"],
Cell[234799, 6241, 634, 15, 89, "Input"],
Cell[235436, 6258, 634, 15, 89, "Input"]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[236119, 6279, 331, 4, 40, "Subsection"],
Cell[236453, 6285, 126, 1, 30, "Text"],
Cell[236582, 6288, 850, 23, 33, "Input"],
Cell[237435, 6313, 349, 9, 31, "Input"],
Cell[237787, 6324, 489, 14, 89, "Input"],
Cell[238279, 6340, 282, 6, 31, "Input"],
Cell[238564, 6348, 451, 12, 89, "Input"],
Cell[CellGroupData[{
Cell[239040, 6364, 375, 9, 52, "Input"],
Cell[239418, 6375, 3555, 71, 47, "Output"],
Cell[242976, 6448, 7871, 190, 120, "Output"]
}, Open  ]],
Cell[250862, 6641, 803, 18, 89, "Input"],
Cell[CellGroupData[{
Cell[251690, 6663, 565, 14, 70, "Input"],
Cell[252258, 6679, 2800, 39, 31, "Output"]
}, Open  ]],
Cell[255073, 6721, 171, 4, 31, "Input"],
Cell[255247, 6727, 110, 1, 30, "Text"],
Cell[CellGroupData[{
Cell[255382, 6732, 117, 2, 31, "Input"],
Cell[255502, 6736, 677, 12, 31, "Output"]
}, Open  ]],
Cell[256194, 6751, 187, 4, 30, "Text"],
Cell[256384, 6757, 130, 3, 23, "Item"]
}, Open  ]],
Cell[CellGroupData[{
Cell[256551, 6765, 108, 1, 78, "Section"],
Cell[256662, 6768, 3144, 93, 621, "Input"],
Cell[259809, 6863, 601, 16, 89, "Input"],
Cell[260413, 6881, 588, 16, 89, "Input"],
Cell[261004, 6899, 662, 17, 89, "Input"],
Cell[261669, 6918, 583, 16, 89, "Input"],
Cell[262255, 6936, 577, 16, 89, "Input"],
Cell[262835, 6954, 579, 16, 89, "Input"],
Cell[263417, 6972, 1131, 32, 184, "Input"],
Cell[264551, 7006, 1166, 35, 184, "Input"],
Cell[CellGroupData[{
Cell[265742, 7045, 91, 1, 40, "Subsection"],
Cell[265836, 7048, 130, 1, 30, "Text"],
Cell[CellGroupData[{
Cell[265991, 7053, 1269, 23, 70, "Input"],
Cell[267263, 7078, 3755, 53, 31, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[271055, 7136, 122, 1, 29, "Subsubsection"],
Cell[271180, 7139, 884, 19, 89, "Input"],
Cell[272067, 7160, 903, 20, 89, "Input"],
Cell[272973, 7182, 624, 16, 90, "Input"],
Cell[273600, 7200, 962, 21, 89, "Input"],
Cell[274565, 7223, 1052, 23, 90, "Input"],
Cell[275620, 7248, 915, 20, 89, "Input"],
Cell[276538, 7270, 637, 17, 89, "Input"],
Cell[277178, 7289, 834, 20, 89, "Input"],
Cell[278015, 7311, 642, 17, 90, "Input"],
Cell[278660, 7330, 783, 19, 90, "Input"],
Cell[279446, 7351, 692, 18, 90, "Input"],
Cell[280141, 7371, 696, 18, 90, "Input"],
Cell[280840, 7391, 637, 17, 89, "Input"],
Cell[281480, 7410, 740, 19, 89, "Input"],
Cell[282223, 7431, 639, 17, 90, "Input"],
Cell[282865, 7450, 741, 19, 90, "Input"],
Cell[283609, 7471, 812, 20, 90, "Input"],
Cell[284424, 7493, 642, 17, 90, "Input"]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[285115, 7516, 91, 1, 40, "Subsection"],
Cell[285209, 7519, 130, 1, 30, "Text"],
Cell[CellGroupData[{
Cell[285364, 7524, 1220, 23, 70, "Input"],
Cell[286587, 7549, 3679, 51, 31, "Output"]
}, Open  ]]
}, Open  ]]
}, Open  ]]
}
]
*)

(* End of internal cache information *)
